From b68d7b7399147bb8cc7071cfe338c27929e328c0 Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Sat, 7 Feb 2026 07:17:55 -0500 Subject: [PATCH] IntakePivot and Shooter changes to make me hate them less, other minor fixes --- .../robot/constants/IntakePivotConstants.java | 12 ++-- .../frc/robot/constants/ShooterConstants.java | 2 + .../frc/robot/subsystems/IntakePivot.java | 29 ++++----- .../java/frc/robot/subsystems/Shooter.java | 63 ++++++++----------- 4 files changed, 46 insertions(+), 60 deletions(-) diff --git a/src/main/java/frc/robot/constants/IntakePivotConstants.java b/src/main/java/frc/robot/constants/IntakePivotConstants.java index dd32568..7fccd7b 100644 --- a/src/main/java/frc/robot/constants/IntakePivotConstants.java +++ b/src/main/java/frc/robot/constants/IntakePivotConstants.java @@ -40,18 +40,18 @@ public class IntakePivotConstants { // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM - public static final SparkMaxConfig leftMotorConfig = new SparkMaxConfig(); - public static final SparkMaxConfig rightMotorConfig = new SparkMaxConfig(); + public static final SparkMaxConfig KLeftMotorConfig = new SparkMaxConfig(); + public static final SparkMaxConfig kRightMotorConfig = new SparkMaxConfig(); static { - leftMotorConfig + KLeftMotorConfig .idleMode(kIdleMode) .smartCurrentLimit(kCurrentLimit) .inverted(kInvertMotors); - leftMotorConfig.absoluteEncoder + KLeftMotorConfig.absoluteEncoder .positionConversionFactor(kConversionFactor) .velocityConversionFactor(kConversionFactor / 60); - leftMotorConfig.closedLoop + KLeftMotorConfig.closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .pid(kP, kI, kD) .outputRange(-1, 1) @@ -60,7 +60,7 @@ public class IntakePivotConstants { .feedForward .sva(kS, kV, kA); - rightMotorConfig + kRightMotorConfig .idleMode(kIdleMode) .smartCurrentLimit(kCurrentLimit) .inverted(kInvertMotors) diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index 0d56175..db4aa15 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -28,6 +28,8 @@ public class ShooterConstants { } } + // TODO Conversion factor? + public static final double kWheelDiameter = Units.inchesToMeters(6); // TODO Real values diff --git a/src/main/java/frc/robot/subsystems/IntakePivot.java b/src/main/java/frc/robot/subsystems/IntakePivot.java index 5cd27ab..d256609 100644 --- a/src/main/java/frc/robot/subsystems/IntakePivot.java +++ b/src/main/java/frc/robot/subsystems/IntakePivot.java @@ -32,13 +32,13 @@ public class IntakePivot extends SubsystemBase { rightMotor = new SparkMax(IntakePivotConstants.kRightMotorCANID, MotorType.kBrushless); leftMotor.configure( - IntakePivotConstants.leftMotorConfig, + IntakePivotConstants.KLeftMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters ); rightMotor.configure( - IntakePivotConstants.rightMotorConfig, + IntakePivotConstants.kRightMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters ); @@ -50,13 +50,6 @@ public class IntakePivot extends SubsystemBase { @Override public void periodic() { - if(currentTargetPosition == null) { - leftMotor.disable(); - rightMotor.disable(); - } else { - controller.setSetpoint(currentTargetPosition.getPositionRadians(), ControlType.kPosition); - } - Logger.recordOutput( "IntakePivot/TargetPosition", currentTargetPosition == null ? -1 : currentTargetPosition.getPositionRadians()); @@ -64,19 +57,23 @@ public class IntakePivot extends SubsystemBase { Logger.recordOutput("IntakePivot/AtSetpoint", controller.isAtSetpoint()); } - public Command setTargetPosition(IntakePivotPosition position) { - return runOnce(() -> { - currentTargetPosition = position; + public Command maintainPosition(IntakePivotPosition position) { + currentTargetPosition = position; + + return run(() -> { + if(currentTargetPosition == null) { + leftMotor.disable(); + } else { + controller.setSetpoint(currentTargetPosition.getPositionRadians(), ControlType.kPosition); + } }); } public Command stop() { - return runOnce(() -> { - currentTargetPosition = null; - }); + return maintainPosition(null); } public Optional getCurrentTargetPosition() { - return Optional.of(currentTargetPosition); + return Optional.ofNullable(currentTargetPosition); } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 804e1e5..1622e0b 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -29,7 +29,7 @@ public class Shooter extends SubsystemBase { private SparkClosedLoopController frontClosedLoopController; private SparkClosedLoopController rearClosedLoopController; - private ShooterSpeeds currentSpeeds; + private ShooterSpeeds targetSpeeds; public Shooter() { frontMotor1 = new SparkMax(ShooterConstants.kFrontShooterMotor1CANID, MotorType.kBrushless); @@ -68,45 +68,19 @@ public class Shooter extends SubsystemBase { rearClosedLoopController = rearMotor1.getClosedLoopController(); // TODO Set this to the initial startup speed - currentSpeeds = null; + targetSpeeds = null; } @Override public void periodic() { - if(currentSpeeds == null) { - if(frontClosedLoopController.getSetpoint() != 0) { - frontClosedLoopController.setSetpoint( - 0, - ControlType.kVelocity - ); - } - - if(rearClosedLoopController.getSetpoint() != 0) { - rearClosedLoopController.setSetpoint( - 0, - ControlType.kVelocity - ); - } - } else { - frontClosedLoopController.setSetpoint( - currentSpeeds.getFrontRollerMPS(), - ControlType.kVelocity - ); - - rearClosedLoopController.setSetpoint( - currentSpeeds.getRearRollerMPS(), - ControlType.kVelocity - ); - } - Logger.recordOutput( "Shooter/FrontRollers/TargetMPS", - currentSpeeds == null ? 0 : currentSpeeds.getFrontRollerMPS() + targetSpeeds == null ? 0 : targetSpeeds.getFrontRollerMPS() ); Logger.recordOutput( "Shooter/RearRollers/TargetMPS", - currentSpeeds == null ? 0 : currentSpeeds.getRearRollerMPS() + targetSpeeds == null ? 0 : targetSpeeds.getRearRollerMPS() ); Logger.recordOutput("Shooter/FrontRollers/CurrentMPS", frontEncoder.getVelocity()); @@ -117,19 +91,32 @@ public class Shooter extends SubsystemBase { Logger.recordOutput("Shooter/RearRollers/AtSetpoint", rearClosedLoopController.isAtSetpoint()); } - public Command setCurrentSpeeds(ShooterSpeeds speeds) { - return runOnce(() -> { - currentSpeeds = speeds; + public Command maintainSpeed(ShooterSpeeds speeds) { + targetSpeeds = speeds; + + return run(() -> { + if(targetSpeeds == null) { + frontMotor1.disable(); + rearMotor1.disable(); + } else { + frontClosedLoopController.setSetpoint( + targetSpeeds.getFrontRollerMPS(), + ControlType.kVelocity + ); + + rearClosedLoopController.setSetpoint( + targetSpeeds.getRearRollerMPS(), + ControlType.kVelocity + ); + } }); } public Command stop() { - return runOnce(() -> { - currentSpeeds = null; - }); + return maintainSpeed(null); } - public Optional getCurrentSpeeds() { - return Optional.ofNullable(currentSpeeds); + public Optional getTargetSpeeds() { + return Optional.ofNullable(targetSpeeds); } }