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.LockedShoot;
if (turret != null) {
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.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.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
- int level = 1;
- double batteryVoltage = RobotController.getBatteryVoltage();
- if (batteryVoltage > BrownOutConstants.LEVEL_ONE_LIMIT) { // normal
- level = 1;
- return levels[0];
- } else if (batteryVoltage > BrownOutConstants.LEVEL_TWO_LIMIT) { // if 7.5 to 6.75
- level = 2;
- return levels[1]; // lower drivetrain
- } else if (batteryVoltage > BrownOutConstants.LEVEL_THREE_LIMIT) { // if 6.75 to 6.0 (browning out)
- level = 3;
- return levels[2];
- } else if (batteryVoltage > BrownOutConstants.LEVEL_FOUR_LIMIT) { // if 6.0 to 5.0 (mayday)
- level = 4;
- return levels[3];
- } else { // were are on life support at this point 5.25 to 4.75
- level = 5;
- return levels[4];
- }
- // Logger.recordOutput("BrownoutProtection/Level", level);
- }
-
- 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
- }
-}
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.constants.Constants;
-import frc.robot.constants.IntakeConstants;
import frc.robot.subsystems.Intake.Intake;
+import frc.robot.subsystems.Intake.IntakeConstants;
import frc.robot.subsystems.hood.Hood;
import frc.robot.subsystems.hood.HoodConstants;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.constants.Constants;
-import frc.robot.constants.IntakeConstants;
import frc.robot.subsystems.Intake.Intake;
+import frc.robot.subsystems.Intake.IntakeConstants;
import frc.robot.subsystems.hood.Hood;
import frc.robot.subsystems.spindexer.Spindexer;
import frc.robot.subsystems.spindexer.SpindexerConstants;
reversing = false;
return; // this is so the balls don't fly out when unaligned
}
- boolean jammed = spindexer.getStatorCurrent() > SpindexerConstants.JAM_CURRENT_THRESHOLD;
+ boolean jammed = spindexer.getSubsystemStatorCurrent() / 2 > SpindexerConstants.JAM_CURRENT_THRESHOLD;
if (jam_debouncer.calculate(jammed)) {
reversing = true;
reverseTimer.reset();
+++ /dev/null
-package frc.robot.constants;
-
-import edu.wpi.first.math.util.Units;
-
-public class IntakeConstants {
- /** Intake roller motor speed in range [-1, 1] */
- public static final double SPEED = 1.0;
- /** 12 tooth pinion driving 36 tooth driven gear */
- public static final double GEAR_RATIO = 48.0/10.0;
- /** radius (inches) of the rack gear which is a 10 tooth pinion at 10 DP */
- public static final double RADIUS_RACK_PINION = 0.5;
- /**right and left motor current limits */
- public static final double EXTENDER_CURRENT_LIMITS = 40.0;
- /**Current limits when calibrating */
- public static final double CALIBRATING_CURRENT_LIMITS = 10.0;
- public static final double CALIBRATING_CURRENT_THRESHOLD = 9.0;
-
- /** Current limit for normal operation */
- 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 INTERMEDIATE_EXTENSION = 5.0; //inches
-
- public static final double STOW_EXTENSION = 0.2; // inches
-
- /** starting point in inches */
- public static final double STARTING_POINT = 0;
- /** rack pitch in teeth per inch of diameter (Diametral Pitch) DP = N teeth / Diameter in inches */
- public static final double RACK_PITCH = 10;
-
- // Simulation
-
-}
hood.forceHoodDown(false);
}));
}
-
- // shoot focus mode: reduces drive current
- if (spindexer != null) {
- controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> shootFocus(true)))
- .onFalse(new InstantCommand(()-> shootFocus(false)));
- }
- }
-
- private void shootFocus(boolean turnOn) {
- if (turnOn) {
- System.out.println("Shooting is now Focused");
- spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit);
- drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT,
- DriveConstants.DRIVE_PEAK_CURRENT_LIMIT * 0.25);
- } else {
- System.out.println("Shooting back to normal (From focus)");
- spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit);
- drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT,
- DriveConstants.DRIVE_PEAK_CURRENT_LIMIT);
- }
}
@Override
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.Constants;
import frc.robot.constants.IdConstants;
-import frc.robot.constants.IntakeConstants;
public class Intake extends SubsystemBase implements IntakeIO{
// Mechanism Display...
maxVelocity = 0.75 * maxFreeSpeed;
maxAcceleration = maxVelocity / 0.25;
+ // ----Rollers
// Configure the motors
// Build the configuration for the roller
TalonFXConfiguration rollerConfig = new TalonFXConfiguration();
// apply the configuration to the right motor (master)
rollerMotor.getConfigurator().apply(rollerConfig);
+ // --- Extenders
+
// Build the configuration for the left and right Motor
TalonFXConfiguration config = new TalonFXConfiguration();
- // config the current limits (low value for testing)
- config.CurrentLimits
- .withStatorCurrentLimit(IntakeConstants.EXTENDER_CURRENT_LIMITS)
- .withStatorCurrentLimitEnable(true)
- .withSupplyCurrentLimit(IntakeConstants.EXTENDER_CURRENT_LIMITS)
- .withSupplyCurrentLimitEnable(true);
-
// config Slot 0 PID params
- var rollerSlot0Configs = config.Slot0;
- rollerSlot0Configs.kP = 0.5;
- rollerSlot0Configs.kI = 0.0;
- rollerSlot0Configs.kD = 0.0;
- rollerSlot0Configs.kV = 0.0;
- rollerSlot0Configs.kA = 0.0;
+ var extenderSlot0Configs = config.Slot0;
+ extenderSlot0Configs.kP = 0.5;
+ extenderSlot0Configs.kI = 0.0;
+ extenderSlot0Configs.kD = 0.0;
+ extenderSlot0Configs.kV = 0.0;
+ extenderSlot0Configs.kA = 0.0;
// configure MotionMagic
MotionMagicConfigs motionMagicConfigs = config.MotionMagic;
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);
+ setNewCurrentLimit(IntakeConstants.STATOR_CURRENT_EXTENDER_LIMIT, IntakeConstants.SUPPLY_CURRENT_EXTENDER_LIMIT, IntakeConstants.STATOR_ROLLER_CURRENT_LIMIT, IntakeConstants.SUPPLY_ROLLER_CURRENT_LIMIT);
+
// Build the mechanism for display
mechanism = new Mechanism2d(80, 80);
MechanismRoot2d root = mechanism.getRoot("Root", 0, 1);
* Starts calibrating by running it backwards
*/
public void calibrate(){
- setCurrentLimits(IntakeConstants.CALIBRATING_CURRENT_LIMITS);
+ setNewCurrentLimit(IntakeConstants.CALIBRATING_CURRENT_LIMITS, IntakeConstants.CALIBRATING_CURRENT_LIMITS, IntakeConstants.CALIBRATING_CURRENT_LIMITS, IntakeConstants.CALIBRATING_CURRENT_LIMITS);
calibrating = true;
}
*/
public void stopCalibrating(){
zeroMotors();
- setCurrentLimits(IntakeConstants.EXTENDER_CURRENT_LIMITS);
+ setNewCurrentLimit(IntakeConstants.STATOR_CURRENT_EXTENDER_LIMIT, IntakeConstants.SUPPLY_CURRENT_EXTENDER_LIMIT, IntakeConstants.STATOR_ROLLER_CURRENT_LIMIT, IntakeConstants.SUPPLY_ROLLER_CURRENT_LIMIT);
calibrating = false;
retract();
}
- /**
- * sets supply and stator current limits
- * @param limit the current limit for stator and supply current
- */
- public void setCurrentLimits(double limit) {
- CurrentLimitsConfigs limits = new CurrentLimitsConfigs()
- .withStatorCurrentLimitEnable(true)
- .withStatorCurrentLimit(limit)
- .withSupplyCurrentLimitEnable(true)
- .withSupplyCurrentLimit(limit);
-
- leftMotor.getConfigurator().apply(limits);
- rightMotor.getConfigurator().apply(limits);
+ public void setNewCurrentLimit(double statorExtenders, double supplyExtenders, double statorRoller, double supplyRollers) {
+ CurrentLimitsConfigs limitConfigExtender = new CurrentLimitsConfigs();
+ limitConfigExtender.StatorCurrentLimit = statorExtenders;
+ limitConfigExtender.StatorCurrentLimitEnable = true;
+ limitConfigExtender.SupplyCurrentLimit = statorExtenders;
+ limitConfigExtender.SupplyCurrentLimitEnable = true;
+ leftMotor.getConfigurator().apply(limitConfigExtender);
+ rightMotor.getConfigurator().apply(limitConfigExtender);
+
+ // roller
+ CurrentLimitsConfigs limitConfigRoller = new CurrentLimitsConfigs();
+ limitConfigRoller.StatorCurrentLimit = statorRoller;
+ limitConfigRoller.StatorCurrentLimitEnable = true;
+ limitConfigRoller.SupplyCurrentLimit = supplyRollers;
+ limitConfigRoller.SupplyCurrentLimitEnable = true;
+ rollerMotor.getConfigurator().apply(limitConfigRoller);
+ }
+
+ public double getSubsystemStatorCurrent() {
+ return inputs.leftStatorCurrent + inputs.rightStatorCurrent + inputs.rollerStatorCurrent;
+ }
+
+ public double getSubsystemSupplyCurrent() {
+ return inputs.leftSupplyCurrent + inputs.rightSupplyCurrent + inputs.rollerSupplyCurrent;
}
@Override
public void updateInputs() {
inputs.leftPosition = rotationsToInches(leftMotor.getPosition().getValueAsDouble());
inputs.rightPosition = rotationsToInches(rightMotor.getPosition().getValueAsDouble());
- inputs.leftCurrent = leftMotor.getStatorCurrent().getValueAsDouble();
- inputs.rightCurrent = rightMotor.getStatorCurrent().getValueAsDouble();
+ inputs.leftStatorCurrent = leftMotor.getStatorCurrent().getValueAsDouble();
+ inputs.rightStatorCurrent = rightMotor.getStatorCurrent().getValueAsDouble();
+ inputs.leftSupplyCurrent = leftMotor.getSupplyCurrent().getValueAsDouble();
+ inputs.rightSupplyCurrent = rightMotor.getSupplyCurrent().getValueAsDouble();
inputs.rollerVelocity = rollerMotor.getVelocity().getValueAsDouble();
- inputs.rollerCurrent = rollerMotor.getStatorCurrent().getValueAsDouble();
+ inputs.rollerStatorCurrent = rollerMotor.getStatorCurrent().getValueAsDouble();
+ inputs.rollerSupplyCurrent = rollerMotor.getSupplyCurrent().getValueAsDouble();
}
}
--- /dev/null
+package frc.robot.subsystems.Intake;
+
+import edu.wpi.first.math.util.Units;
+
+public class IntakeConstants {
+ /** Intake roller motor speed in range [-1, 1] */
+ public static final double SPEED = 1.0;
+ /** 12 tooth pinion driving 36 tooth driven gear */
+ public static final double GEAR_RATIO = 48.0/10.0;
+ /** radius (inches) of the rack gear which is a 10 tooth pinion at 10 DP */
+ public static final double RADIUS_RACK_PINION = 0.5;
+
+ /**Current limits when calibrating */
+ public static final double CALIBRATING_CURRENT_LIMITS = 10.0;
+ public static final double CALIBRATING_CURRENT_THRESHOLD = 9.0;
+
+ /** Current limit for normal operation */
+ public static final double STATOR_CURRENT_EXTENDER_LIMIT = 100.0;
+ public static final double SUPPLY_CURRENT_EXTENDER_LIMIT = 100.0;
+ public static final double STATOR_ROLLER_CURRENT_LIMIT = 40.0;
+ public static final double SUPPLY_ROLLER_CURRENT_LIMIT = 40.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 INTERMEDIATE_EXTENSION = 5.0; //inches
+
+ public static final double STOW_EXTENSION = 0.2; // inches
+
+ /** starting point in inches */
+ public static final double STARTING_POINT = 0;
+ /** rack pitch in teeth per inch of diameter (Diametral Pitch) DP = N teeth / Diameter in inches */
+ public static final double RACK_PITCH = 10;
+
+ // Simulation
+
+}
public static class IntakeIOInputs {
public double leftPosition = 0.0;
public double rightPosition = 0.0;
- public double leftCurrent = 0.0;
- public double rightCurrent = 0.0;
+ public double leftSupplyCurrent = 0.0;
+ public double rightSupplyCurrent = 0.0;
+ public double leftStatorCurrent = 0.0;
+ public double rightStatorCurrent = 0.0;
public double rollerVelocity = 0.0;
- public double rollerCurrent = 0.0;
+ public double rollerSupplyCurrent = 0.0;
+ public double rollerStatorCurrent = 0.0;
}
public void updateInputs();
}
// for current limit setting (brownout protection)
- public void applyNewModuleCurrents(double steerCurrent, double driveCurrent) {
+ public void applyNewModuleCurrents(
+ double steerCurrentStator, double steerCurrentSupply,
+ double driveCurrentStator, double driveCurrentSupply) {
for (Module module : modules) { // iterate over our modules
- module.setNewCurrentLimit(steerCurrent, driveCurrent);
+ module.setNewCurrentLimit(steerCurrentStator, steerCurrentSupply, driveCurrentStator, driveCurrentSupply);
}
}
+
+ public double getSubsystemStatorCurrent() {
+ double sum = 0;
+ for (Module module : modules) {
+ sum += module.getModuleStatorCurrent();
+ }
+ return sum;
+ }
+
+ public double getSubsystemSupplyCurrent() {
+ double sum = 0;
+ for (Module module : modules) {
+ sum += module.getModuleSupplyCurrent();
+ }
+ return sum;
+ }
/**
* Sets the desired states for all swerve modules.
// Update encoder inputs
inputs.encoderOffset = Units.rotationsToDegrees(CANcoder.getAbsolutePosition().getValueAsDouble());
- // Update odometry inputs
- inputs.odometryTimestamps =
- timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
- inputs.odometryDrivePositionsRad =
- drivePositionQueue.stream()
- .mapToDouble((Double value) -> Units.rotationsToRadians(value))
- .toArray();
- inputs.odometryTurnPositions =
- turnPositionQueue.stream()
- .map((Double value) -> Rotation2d.fromRotations(value))
- .toArray(Rotation2d[]::new);
- timestampQueue.clear();
- drivePositionQueue.clear();
- turnPositionQueue.clear();
-
+ // Update odometry inputs
+ inputs.odometryTimestamps =
+ timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
+ inputs.odometryDrivePositionsRad =
+ drivePositionQueue.stream()
+ .mapToDouble((Double value) -> Units.rotationsToRadians(value))
+ .toArray();
+ inputs.odometryTurnPositions =
+ turnPositionQueue.stream()
+ .map((Double value) -> Rotation2d.fromRotations(value))
+ .toArray(Rotation2d[]::new);
+ timestampQueue.clear();
+ drivePositionQueue.clear();
+ turnPositionQueue.clear();
+
+ inputs.driveStator = driveMotor.getStatorCurrent().getValueAsDouble();
+ inputs.driveSupply = driveMotor.getSupplyCurrent().getValueAsDouble();
+ inputs.steerStator = angleMotor.getStatorCurrent().getValueAsDouble();
+ inputs.steerSupply = angleMotor.getSupplyCurrent().getValueAsDouble();
}
public void periodic() {
return inputs.driveCurrentAmps;
}
+ public double getModuleStatorCurrent() {
+ return inputs.steerStator + inputs.driveStator;
+ }
+
+ public double getModuleSupplyCurrent() {
+ return inputs.steerSupply + inputs.driveSupply;
+ }
+
// I took the config things straight from this file
- public void setNewCurrentLimit(double currentSteer, double currentDrive) {
+ public void setNewCurrentLimit(double currentSteerStator, double currentSteerSupply, double currentDriveStator, double currentDriveSupply) {
CurrentLimitsConfigs steerConfig = new CurrentLimitsConfigs();
// steer
- steerConfig.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT;
- steerConfig.SupplyCurrentLimit = currentSteer;
- steerConfig.SupplyCurrentLowerLimit = currentSteer;
+ steerConfig.SupplyCurrentLimitEnable = true;
+ steerConfig.StatorCurrentLimitEnable = true;
+ steerConfig.StatorCurrentLimit = currentSteerSupply;
+ steerConfig.SupplyCurrentLimit = currentSteerSupply;
steerConfig.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
angleMotor.getConfigurator().apply(steerConfig); // apply
// drive
CurrentLimitsConfigs driveConfig = new CurrentLimitsConfigs();
- driveConfig.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
- driveConfig.SupplyCurrentLimit = currentDrive;
- driveConfig.SupplyCurrentLowerLimit = currentDrive;
+ driveConfig.SupplyCurrentLimitEnable = true;
+ driveConfig.StatorCurrentLimitEnable = true;
+ driveConfig.SupplyCurrentLimit = currentDriveSupply;
+ driveConfig.StatorCurrentLimit = currentDriveStator;
driveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
- driveConfig.StatorCurrentLimit = currentDrive;
- driveConfig.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
driveMotor.getConfigurator().apply(driveConfig); // apply
}
public Rotation2d[] odometryTurnPositions = new Rotation2d[] {};
public double encoderOffset = 0.0;
+
+ // drivetrain is scary. I'm adding my own seperate logging
+ public double driveStator = 0.0;
+ public double driveSupply = 0.0;
+ public double steerStator = 0.0;
+ public double steerSupply = 0.0;
}
/** Updates the set of loggable inputs. */
mm.MotionMagicJerk = 0; // Set to > 0 for "S-Curve" smoothing if needed
motor.getConfigurator().apply(config);
- setCurrentLimits(HoodConstants.NORMAL_CURRENT_LIMIT);
+ setNewCurrentLimit(HoodConstants.STATOR_CURRENT_LIMIT, HoodConstants.SUPPLY_CURRENT_LIMIT);
motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO);
public void calibrate(){
calibrating = true;
- setCurrentLimits(HoodConstants.CALIBRATING_CURRENT_LIMIT);
+ setNewCurrentLimit(HoodConstants.CALIBRATING_CURRENT_LIMIT, HoodConstants.CALIBRATING_CURRENT_LIMIT);
}
public void stopCalibrating(){
motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO);
goalAngle = new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE));
- setCurrentLimits(HoodConstants.NORMAL_CURRENT_LIMIT);
+ setNewCurrentLimit(HoodConstants.STATOR_CURRENT_LIMIT, HoodConstants.SUPPLY_CURRENT_LIMIT);
calibrating = false;
}
- /**
- * sets supply and stator current limits
- * @param limitAmps the current limit for stator and supply current
- */
- public void setCurrentLimits(double limitAmps) {
- CurrentLimitsConfigs limits = new CurrentLimitsConfigs()
- .withStatorCurrentLimitEnable(true)
- .withStatorCurrentLimit(limitAmps)
- .withSupplyCurrentLimitEnable(true)
- .withSupplyCurrentLimit(limitAmps);
-
- motor.getConfigurator().apply(limits);
+ public void setNewCurrentLimit(double stator, double supply) {
+ CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
+ limitConfig.StatorCurrentLimit = stator;
+ limitConfig.StatorCurrentLimitEnable = true;
+ limitConfig.SupplyCurrentLimit = supply;
+ limitConfig.SupplyCurrentLimitEnable = true;
+ motor.getConfigurator().apply(limitConfig);
+ }
+
+ public double getSubsystemStatorCurrent() {
+ return inputs.motorStatorCurrent;
+ }
+
+ public double getSubsystemSupplyCurrent() {
+ return inputs.motorSupplyCurrent;
}
@Override
public void updateInputs() {
inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / HoodConstants.HOOD_GEAR_RATIO;
inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / HoodConstants.HOOD_GEAR_RATIO;
- inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble();
+ inputs.motorStatorCurrent = motor.getStatorCurrent().getValueAsDouble();
+ inputs.motorSupplyCurrent = motor.getStatorCurrent().getValueAsDouble();
}
}
public static final double FEEDFORWARD_KV = 0.12;
- public static final double NORMAL_CURRENT_LIMIT = 40.0; // A
+ public static final double STATOR_CURRENT_LIMIT = 40.0; // A
+ public static final double SUPPLY_CURRENT_LIMIT = 40.0; // A
public static final double CALIBRATING_CURRENT_LIMIT = 10.0; // A
public static final double CALIBRATION_CURRENT_THRESHOLD = 9.0; // A
}
public static class HoodIOInputs{
public double positionDeg = HoodConstants.MAX_ANGLE;
public double velocityRadPerSec = 0.0;
- public double motorCurrent = 0.0;
+ public double motorStatorCurrent = 0.0;
+ public double motorSupplyCurrent = 0.0;
}
public void updateInputs();
return inputs.shooterSpeedLeft;
}
- public void setNewCurrentLimit(double newCurrentLimit) {
+ public void setNewCurrentLimit(double stator, double supply) {
CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
- limitConfig.StatorCurrentLimit = newCurrentLimit;
+ limitConfig.StatorCurrentLimit = stator;
limitConfig.StatorCurrentLimitEnable = true;
+ limitConfig.SupplyCurrentLimit = supply;
+ limitConfig.SupplyCurrentLimitEnable = true;
shooterMotorLeft.getConfigurator().apply(limitConfig);
shooterMotorRight.getConfigurator().apply(limitConfig);
}
+ public double getSubsystemStatorCurrent() {
+ return inputs.shooterStatorCurrentLeft + inputs.shooterStatorCurrentRight;
+ }
+
+ public double getSubsystemSupplyCurrent() {
+ return inputs.shooterSupplyCurrentLeft + inputs.shooterSupplyCurrentRight;
+ }
+
@Override
public void updateInputs(){
inputs.shooterSpeedLeft = Units.rotationsToRadians(shooterMotorLeft.getVelocity().getValueAsDouble()) * ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2;
inputs.shooterSpeedRight = Units.rotationsToRadians(shooterMotorRight.getVelocity().getValueAsDouble())* ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2;
- inputs.shooterCurrentLeft = shooterMotorLeft.getStatorCurrent().getValueAsDouble();
- inputs.shooterCurrentRight = shooterMotorRight.getStatorCurrent().getValueAsDouble();
-
+ inputs.shooterStatorCurrentLeft = shooterMotorLeft.getStatorCurrent().getValueAsDouble();
+ inputs.shooterStatorCurrentRight = shooterMotorRight.getStatorCurrent().getValueAsDouble();
+ inputs.shooterSupplyCurrentLeft = shooterMotorLeft.getSupplyCurrent().getValueAsDouble();
+ inputs.shooterSupplyCurrentRight = shooterMotorRight.getSupplyCurrent().getValueAsDouble();
Logger.processInputs("Shooter", inputs);
}
public static class ShooterIOInputs {
public double shooterSpeedLeft = 0.0;
public double shooterSpeedRight = 0.0;
- public double shooterCurrentLeft = 0.0;
- public double shooterCurrentRight = 0.0;
+ public double shooterStatorCurrentLeft = 0.0;
+ public double shooterStatorCurrentRight = 0.0;
+ public double shooterSupplyCurrentLeft = 0.0;
+ public double shooterSupplyCurrentRight = 0.0;
}
public void updateInputs();
state = SpindexerState.CUSTOM;
}
- public double getStatorCurrent() {
- return inputs.spindexerOneCurrent + inputs.spindexerTwoCurrent;
- }
-
- public void setNewCurrentLimit(double newCurrentLimit) {
+ public void setNewCurrentLimit(double stator, double supply) {
CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
- limitConfig.StatorCurrentLimit = newCurrentLimit;
+ limitConfig.StatorCurrentLimit = stator;
limitConfig.StatorCurrentLimitEnable = true;
- limitConfig.SupplyCurrentLowerLimit = newCurrentLimit;
- limitConfig.SupplyCurrentLowerTime = 1.5;
+ limitConfig.SupplyCurrentLimit = supply;
+ limitConfig.SupplyCurrentLimitEnable = true;
motorOne.getConfigurator().apply(limitConfig);
motorTwo.getConfigurator().apply(limitConfig);
}
+ public double getSubsystemStatorCurrent() {
+ return inputs.spindexerOneStatorCurrent + inputs.spindexerTwoStatorCurrent;
+ }
+
+ public double getSubsystemSupplyCurrent() {
+ return inputs.spindexerOneSupplyCurrent + inputs.spindexerTwoSupplyCurrent;
+ }
+
@Override
public void updateInputs() {
inputs.spindexerOneVelocity = motorOne.getVelocity().getValueAsDouble();
- inputs.spindexerOneCurrent = motorOne.getStatorCurrent().getValueAsDouble();
+ inputs.spindexerOneStatorCurrent = motorOne.getStatorCurrent().getValueAsDouble();
+ inputs.spindexerOneSupplyCurrent = motorOne.getSupplyCurrent().getValueAsDouble();
inputs.spindexerTwoVelocity = motorTwo.getVelocity().getValueAsDouble();
- inputs.spindexerTwoCurrent = motorTwo.getStatorCurrent().getValueAsDouble();
+ inputs.spindexerTwoStatorCurrent = motorTwo.getStatorCurrent().getValueAsDouble();
+ inputs.spindexerTwoSupplyCurrent = motorTwo.getSupplyCurrent().getValueAsDouble();
}
}
@AutoLog
public static class SpindexerIOInputs {
public double spindexerOneVelocity = 0.0;
- public double spindexerOneCurrent = 0.0;
+ public double spindexerOneSupplyCurrent = 0.0;
+ public double spindexerOneStatorCurrent = 0.0;
public double spindexerTwoVelocity = 0.0;
- public double spindexerTwoCurrent = 0.0;
+ public double spindexerTwoStatorCurrent = 0.0;
+ public double spindexerTwoSupplyCurrent = 0.0;
}
public void updateInputs();
private double lastFilteredRad = 0.0;
private double lastRawSetpoint = 0.0;
-
-
/* ---------------- Visualization ---------------- */
private final Mechanism2d mech = new Mechanism2d(100, 100);
private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(0);
- ModifiedCRT crt;
-
/* ---------------- Constructor ---------------- */
public Turret() {
mm.MotionMagicJerk = 0; // Set to > 0 for "S-Curve" smoothing if needed
motor.getConfigurator().apply(config);
- setCurrentLimits(TurretConstants.NORMAL_CURRENT_LIMIT);
+ setNewCurrentLimit(TurretConstants.STATOR_CURRENT_LIMIT, TurretConstants.SUPPLY_CURRENT_LIMIT);
lastGoalRad = 0.0;
if (!Constants.DISABLE_SMART_DASHBOARD) {
SmartDashboard.putData("Turret Mech", mech);
- SmartDashboard.putData("Start turret calibration", new InstantCommand(()-> calibrate()));
- SmartDashboard.putData("Stop turret calibration", new InstantCommand(()-> stopCalibrating()));
SmartDashboard.putData("Reset Turret Position to Zero", new InstantCommand(() -> resetTurretPosition()));
SendableChooser<InstantCommand> turretTestChooser = new SendableChooser<>();
// Multiply goal velocity by kV
double robotTurnCompensation = goalVelocityRadPerSec * TurretConstants.FEEDFORWARD_KV * TurretConstants.GEAR_RATIO;
- if(calibrating){
- motor.set(0.05);
- boolean calibrated = Math.abs(motor.getStatorCurrent().getValueAsDouble()) >= TurretConstants.CALIBRATION_CURRENT_THRESHOLD;
- if(calibrationDebouncer.calculate(calibrated)){
- stopCalibrating();
- }
- } else {
- // Sets motor control with feedforward
- motor.setControl(mmVoltageRequest
- .withPosition(motorGoalRotations)
- .withFeedForward(robotTurnCompensation)
- .withEnableFOC(true));
- }
+ // Sets motor control with feedforward
+ motor.setControl(mmVoltageRequest
+ .withPosition(motorGoalRotations)
+ .withFeedForward(robotTurnCompensation)
+ .withEnableFOC(true));
if (!Constants.DISABLE_LOGGING) {
Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue());
SmartDashboard.putBoolean("Turret Calibrated", !calibrating);
SmartDashboard.putBoolean("Turret At Setpoint", atSetpoint());
}
-
}
/* ---------------- Simulation ---------------- */
public void updateInputs() {
inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO;
inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / TurretConstants.GEAR_RATIO;
- inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble();
+ inputs.motorStatorCurrent = motor.getStatorCurrent().getValueAsDouble();
+ inputs.motorSupplyCurrent = motor.getSupplyCurrent().getValueAsDouble();
inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble();
}
- /**
- * sets supply and stator current limits
- * @param limit the current limit for stator and supply current
- */
- public void setCurrentLimits(double limit) {
- CurrentLimitsConfigs limits = new CurrentLimitsConfigs()
- .withStatorCurrentLimitEnable(true)
- .withStatorCurrentLimit(limit)
- .withSupplyCurrentLimitEnable(true)
- .withSupplyCurrentLimit(limit);
-
- if(limit == TurretConstants.NORMAL_CURRENT_LIMIT){
- limits.SupplyCurrentLowerLimit = TurretConstants.CALIBRATION_CURRENT_LIMIT;
- limits.SupplyCurrentLowerTime = 1.0; // Set to lower limit if over 1 second
- }
-
- motor.getConfigurator().apply(limits);
+ public void setNewCurrentLimit(double stator, double supply) {
+ CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
+ limitConfig.StatorCurrentLimit = stator;
+ limitConfig.StatorCurrentLimitEnable = true;
+ limitConfig.SupplyCurrentLimit = supply;
+ limitConfig.SupplyCurrentLimitEnable = true;
+ motor.getConfigurator().apply(limitConfig);
}
- // Also ignore this for now
- private double wrapUnit(double value) {
- value %= 1.0;
- if (value < 0) value += 1.0;
- return value;
- }
-
- private void calibrate(){
- setCurrentLimits(TurretConstants.CALIBRATION_CURRENT_LIMIT);
- calibrating = true;
- }
+ public double getSubsystemStatorCurrent() {
+ return inputs.motorStatorCurrent;
+ }
- private void stopCalibrating(){
- motor.set(Units.degreesToRotations(TurretConstants.CALIBRATION_OFFSET) * TurretConstants.GEAR_RATIO);
- setCurrentLimits(TurretConstants.NORMAL_CURRENT_LIMIT);
- calibrating = false;
- setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(TurretConstants.CALIBRATION_OFFSET)), 0.0);
- }
+ public double getSubsystemSupplyCurrent() {
+ return inputs.motorSupplyCurrent;
+ }
}
public static final double FEEDFORWARD_KV = 0.06;
- public static final double NORMAL_CURRENT_LIMIT = 40.0; // A
- public static final double CALIBRATION_CURRENT_LIMIT = 10.0; // A
- public static final double CALIBRATION_CURRENT_THRESHOLD = 9.0; // A
+ public static final double STATOR_CURRENT_LIMIT = 40.0; // A
+ public static final double SUPPLY_CURRENT_LIMIT = 40.0; // A
}
public static class TurretIOInputs{
public double positionDeg = 0;
public double velocityRadPerSec = 0;
- public double motorCurrent = 0;
+ public double motorStatorCurrent = 0;
+ public double motorSupplyCurrent = 0;
public double encoderLeftRot = 0;
public double encoderRightRot = 0;
public double motorVoltage = 0;
+++ /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
-
- // level numbers
- public static final double LEVEL_ONE_LIMIT = 7.5;
- public static final double LEVEL_TWO_LIMIT = 6.75;
- public static final double LEVEL_THREE_LIMIT = 6.0;
- public static final double LEVEL_FOUR_LIMIT = 5.25;
-
- // 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
- );
-
-}
+++ /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 spindexerCurrent,
- double turretCurrent,
- double intakeCurrent,
- double steerCurrent,
- double driveCurrent
- ) {
- this.shooterCurrent = shooterCurrent;
- this.hoodCurrent = hoodCurrent;
- this.spindexerCurrent = spindexerCurrent;
- this.turretCurrent = turretCurrent;
- this.intakeCurrent = intakeCurrent;
- this.steerCurrent = steerCurrent;
- this.driveCurrent = driveCurrent;
- }
-
-}
-