diff --git a/src/main/java/com/cyberknights4911/robot2024/Robot2024.java b/src/main/java/com/cyberknights4911/robot2024/Robot2024.java index 2d8f878..3570c41 100644 --- a/src/main/java/com/cyberknights4911/robot2024/Robot2024.java +++ b/src/main/java/com/cyberknights4911/robot2024/Robot2024.java @@ -25,6 +25,8 @@ import com.cyberknights4911.robot2024.collect.CollectIOSim; import com.cyberknights4911.robot2024.control.ControllerBinding; import com.cyberknights4911.robot2024.drive.ModuleIOSparkFlex; +import com.cyberknights4911.robot2024.indexer.Indexer; +import com.cyberknights4911.robot2024.indexer.IndexerIO; import com.cyberknights4911.robot2024.shooter.Shooter; import com.cyberknights4911.robot2024.shooter.ShooterIO; import com.cyberknights4911.robot2024.shooter.ShooterIOSim; @@ -35,6 +37,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import org.littletonrobotics.junction.LoggedRobot; @@ -43,6 +46,7 @@ public final class Robot2024 implements RobotContainer { private final Climb climb; private final Collect collect; + private final Indexer indexer; private final Shooter shooter; private final Drive drive; private final Constants constants; @@ -54,6 +58,7 @@ public Robot2024() { burnManager = new SparkBurnManager(constants); climb = createClimb(); collect = createCollect(); + indexer = createIndexer(); shooter = createShooter(); drive = createDrive(); @@ -78,6 +83,7 @@ private void configureControls() { binding.supplierFor(StickAction.STRAFE), binding.supplierFor(StickAction.ROTATE))); + // TODO: Maybe remove this or make it harder to do by accident. binding.triggersFor(ButtonAction.ZeroGyro).onTrue(drive.zeroPoseToCurrentRotation()); binding @@ -101,6 +107,8 @@ private void configureControls() { binding.supplierFor(StickAction.FORWARD), binding.supplierFor(StickAction.STRAFE), Math.PI / 2)); + + // TODO: combine with auto-aim. binding .triggersFor(ButtonAction.SpeakerLockOn) .whileTrue( @@ -113,13 +121,29 @@ private void configureControls() { binding.triggersFor(ButtonAction.StowShooter).onTrue(Commands.none()); - binding.triggersFor(ButtonAction.CollectNote).onTrue(Commands.none()); + binding.triggersFor(ButtonAction.CollectNote).onTrue(collectNote()); binding.triggersFor(ButtonAction.FireNote).onTrue(Commands.none()); binding.triggersFor(ButtonAction.StowClimber).onTrue(climb.climb(drive)); - binding.triggersFor(ButtonAction.ExtendClimber).onTrue(Commands.none()); + binding.triggersFor(ButtonAction.ExtendClimber).onTrue(climb.extendClimber()); + } + + private Command collectNote() { + Command stowShooter = shooter.stowShooter(); + Command runIndexer = indexer.runIndexUntilSensor(); + Command collectNote = + collect.collectAtTunableSpeed( + () -> { + /* TODO: notify drive team that note is secure (lights & rumble) */ + }); + + return Commands.parallel(stowShooter, runIndexer, collectNote) + .andThen( + () -> { + /* TODO: notify drive team that we're ready to fire (probably rumble only) */ + }); } @Override @@ -127,25 +151,11 @@ public void onRobotPeriodic(LoggedRobot robot) { binding.checkControllers(); if (GameAlerts.shouldAlert(GameAlerts.Endgame1)) { - CommandScheduler.getInstance() - .schedule( - Commands.runOnce(() -> binding.setDriverRumble(true)) - .withTimeout(1.5) - .andThen(() -> binding.setDriverRumble(false)) - .withTimeout(1.0)); + CommandScheduler.getInstance().schedule(rumbleLongOnce()); } if (GameAlerts.shouldAlert(GameAlerts.Endgame2)) { - CommandScheduler.getInstance() - .schedule( - Commands.runOnce(() -> binding.setDriverRumble(true)) - .withTimeout(1.0) - .andThen(() -> binding.setDriverRumble(false)) - .withTimeout(0.5) - .andThen(() -> binding.setDriverRumble(true)) - .withTimeout(1.0) - .andThen(() -> binding.setDriverRumble(false)) - .withTimeout(0.5)); + CommandScheduler.getInstance().schedule(rumbleMediumTwice()); } } @@ -181,6 +191,17 @@ private Collect createCollect() { } } + private Indexer createIndexer() { + switch (constants.mode()) { + case SIM: + return new Indexer(SimRobot2024Constants.INDEXER_CONSTANTS, new IndexerIO() {}); + case REAL: + case REPLAY: + default: + return new Indexer(Robot2024Constants.INDEXER_CONSTANTS, new IndexerIO() {}); + } + } + private Shooter createShooter() { switch (constants.mode()) { case SIM: @@ -238,6 +259,35 @@ private Drive createDrive() { new ModuleIO() {}); } } + + private Command rumbleLongOnce() { + return Commands.runOnce(() -> binding.setDriverRumble(true)) + .withTimeout(1.5) + .andThen(() -> binding.setDriverRumble(false)) + .withTimeout(1.0); + } + + private Command rumbleMediumTwice() { + return Commands.runOnce(() -> binding.setDriverRumble(true)) + .withTimeout(1.0) + .andThen(() -> binding.setDriverRumble(false)) + .withTimeout(0.5) + .andThen(() -> binding.setDriverRumble(true)) + .withTimeout(1.0) + .andThen(() -> binding.setDriverRumble(false)) + .withTimeout(0.5); + } + + private Command rumbleQuickTwice() { + return Commands.runOnce(() -> binding.setDriverRumble(true)) + .withTimeout(0.5) + .andThen(() -> binding.setDriverRumble(false)) + .withTimeout(0.25) + .andThen(() -> binding.setDriverRumble(true)) + .withTimeout(0.5) + .andThen(() -> binding.setDriverRumble(false)) + .withTimeout(0.25); + } } // TODO: diff --git a/src/main/java/com/cyberknights4911/robot2024/Robot2024Constants.java b/src/main/java/com/cyberknights4911/robot2024/Robot2024Constants.java index 3e603fa..b8979ef 100644 --- a/src/main/java/com/cyberknights4911/robot2024/Robot2024Constants.java +++ b/src/main/java/com/cyberknights4911/robot2024/Robot2024Constants.java @@ -19,6 +19,8 @@ import com.cyberknights4911.robot2024.climb.ClimbConstantsBuilder; import com.cyberknights4911.robot2024.collect.CollectConstants; import com.cyberknights4911.robot2024.collect.CollectConstantsBuilder; +import com.cyberknights4911.robot2024.indexer.IndexerConstants; +import com.cyberknights4911.robot2024.indexer.IndexerConstantsBuilder; import com.cyberknights4911.robot2024.shooter.ShooterConstants; import com.cyberknights4911.robot2024.shooter.ShooterConstantsBuilder; import com.cyberknights4911.util.FeedForwardValues; @@ -106,13 +108,19 @@ private Robot2024Constants() {} public static CollectConstants COLLECT_CONSTANTS = CollectConstantsBuilder.builder() .motorCollectId(11) - .motorGuideId(12) .sensorId(0) .solenoidLeftId(0) .solenoidRightId(0) .collectGearRatio(24.0 / 18.0) .collectFeedBackValues(new PidValues(0, 0, 0)) - .guideFeedBackValues(new PidValues(0, 0, 0)) + .build(); + + public static IndexerConstants INDEXER_CONSTANTS = + IndexerConstantsBuilder.builder() + .motorId(31) + .sensorId(0) + .speed(100) + .feedBackValues(new PidValues(0, 0, 0)) .build(); public static ShooterConstants SHOOTER_CONSTANTS = diff --git a/src/main/java/com/cyberknights4911/robot2024/SimRobot2024Constants.java b/src/main/java/com/cyberknights4911/robot2024/SimRobot2024Constants.java index 39a8210..c78a120 100644 --- a/src/main/java/com/cyberknights4911/robot2024/SimRobot2024Constants.java +++ b/src/main/java/com/cyberknights4911/robot2024/SimRobot2024Constants.java @@ -19,6 +19,8 @@ import com.cyberknights4911.robot2024.climb.ClimbConstantsBuilder; import com.cyberknights4911.robot2024.collect.CollectConstants; import com.cyberknights4911.robot2024.collect.CollectConstantsBuilder; +import com.cyberknights4911.robot2024.indexer.IndexerConstants; +import com.cyberknights4911.robot2024.indexer.IndexerConstantsBuilder; import com.cyberknights4911.robot2024.shooter.ShooterConstants; import com.cyberknights4911.robot2024.shooter.ShooterConstantsBuilder; import com.cyberknights4911.util.FeedForwardValues; @@ -99,13 +101,19 @@ private SimRobot2024Constants() {} public static CollectConstants COLLECT_CONSTANTS = CollectConstantsBuilder.builder() .motorCollectId(11) - .motorGuideId(12) .sensorId(0) .solenoidLeftId(0) .solenoidRightId(0) .collectGearRatio(24.0 / 18.0) .collectFeedBackValues(new PidValues(0, 0, 0)) - .guideFeedBackValues(new PidValues(0, 0, 0)) + .build(); + + public static IndexerConstants INDEXER_CONSTANTS = + IndexerConstantsBuilder.builder() + .motorId(31) + .sensorId(0) + .speed(100) + .feedBackValues(new PidValues(0, 0, 0)) .build(); public static ShooterConstants SHOOTER_CONSTANTS = diff --git a/src/main/java/com/cyberknights4911/robot2024/collect/Collect.java b/src/main/java/com/cyberknights4911/robot2024/collect/Collect.java index 1d1c9db..a79e343 100644 --- a/src/main/java/com/cyberknights4911/robot2024/collect/Collect.java +++ b/src/main/java/com/cyberknights4911/robot2024/collect/Collect.java @@ -24,18 +24,14 @@ public class Collect extends SubsystemBase { private static final LoggedTunableNumber beamThreshold = - new LoggedTunableNumber("Collect/beamThreshold"); + new LoggedTunableNumber("Collect/BeamThreshold"); private static final LoggedTunableNumber ejectSpeed = new LoggedTunableNumber("Collect/EjectVelocityRPM"); private static final LoggedTunableNumber collectSpeed = new LoggedTunableNumber("Collect/CollectVelocityRPM"); - private static final LoggedTunableNumber feedShooterSpeed = - new LoggedTunableNumber("Collect/FeedShooterVelocityRPM"); private static final LoggedTunableNumber collectKp = new LoggedTunableNumber("Collect/CollectKp"); private static final LoggedTunableNumber collectKd = new LoggedTunableNumber("Collect/CollectKd"); private static final LoggedTunableNumber ejectTime = new LoggedTunableNumber("Collect/EjectTime"); - private static final LoggedTunableNumber guideKp = new LoggedTunableNumber("Collect/GuideKp"); - private static final LoggedTunableNumber guideKd = new LoggedTunableNumber("Collect/GuideKd"); // Measured in OnShape private static final Translation2d SEGMENT_1_START = new Translation2d(3.267, 15.613); @@ -68,14 +64,10 @@ public Collect(CollectConstants constants, CollectIO collectIO) { this.collectIO = collectIO; collectKp.initDefault(constants.collectFeedBackValues().kP()); collectKd.initDefault(constants.collectFeedBackValues().kD()); - feedShooterSpeed.initDefault(constants.feedShooterSpeed()); collectSpeed.initDefault(constants.collectSpeed()); ejectSpeed.initDefault(constants.ejectSpeed()); ejectTime.initDefault(constants.ejectTime()); - guideKp.initDefault(constants.guideFeedBackValues().kP()); - guideKd.initDefault(constants.guideFeedBackValues().kP()); collectIO.configureCollectPID(collectKp.get(), 0.0, collectKd.get()); - collectIO.configureGuidePID(guideKp.get(), 0.0, guideKd.get()); mechanism = new Mechanism2d(28.0, 28.0); // Where the collector is attached to the frame @@ -113,18 +105,6 @@ public void runCollectVelocity(double velocityRpm) { Logger.recordOutput("Collect/SetpointRPM", velocityRpm); } - /** Run guide closed loop at the specified velocity. */ - public void runGuideVelocity(double velocityRpm) { - var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRpm); - collectIO.setGuideVelocity(velocityRadPerSec); - - Logger.recordOutput("Collect/GuideSetpointRPM", velocityRpm); - } - - public void stopGuide() { - collectIO.stopGuide(); - } - public void extendCollecter() { collectIO.setCollecterPosition(true); } @@ -142,10 +122,6 @@ public void periodic() { collectIO.configureCollectPID(collectKp.get(), 0.0, collectKd.get()); } - if (guideKp.hasChanged(hashCode()) || guideKd.hasChanged(hashCode())) { - collectIO.configureGuidePID(guideKp.get(), 0.0, guideKd.get()); - } - if (inputs.leftSolenoid && inputs.rightSolenoid) { segment1.setAngle(SEGMENT_1_END_ANGLE); segment2.setAngle(SEGMENT_2_END_ANGLE); @@ -160,7 +136,6 @@ public void periodic() { if (DriverStation.isDisabled()) { stopCollector(); - stopGuide(); } } @@ -204,6 +179,21 @@ private Command collectAtSpeed(LoggedTunableNumber desiredSpeed) { this); } + /** + * Creates a command that runs the collector at the proper collector speed. The collector will + * stay running after this command ends, but the provided callback will be notified when the + * gamepiece reaches the collector's sensor. + * + * @param sensorTrippedCallback will notify when the gamepiece reaches the sensor. At this point, + * the gamepiece is secure in the robot and the driver should be able to move the robot freely + * while the gamepiece moves through the robot. + */ + public Command collectAtTunableSpeed(Runnable sensorTrippedCallback) { + return Commands.runOnce(() -> runCollectVelocity(collectSpeed.get()), this) + .until(this::isBeamBreakBlocked) + .andThen(() -> sensorTrippedCallback.run()); + } + /** * Creates a command that runs the collector at the proper collector speed, but stops when the * gamepiece reaches the sensor. @@ -214,11 +204,6 @@ public Command collectGamePieceAndStop() { .andThen(this::stopCollector, this); } - /** Creates a command that runs the collector in the mode for handing gamepieces to the shooter */ - public Command handGamePieceToShooter() { - return collectAtSpeed(feedShooterSpeed); - } - /** Creates a command for ejecting gamepieces backwards, out of the collector. */ public Command ejectGamePiece() { return collectAtSpeed(ejectSpeed) diff --git a/src/main/java/com/cyberknights4911/robot2024/collect/CollectConstants.java b/src/main/java/com/cyberknights4911/robot2024/collect/CollectConstants.java index b5563a3..a365a0d 100644 --- a/src/main/java/com/cyberknights4911/robot2024/collect/CollectConstants.java +++ b/src/main/java/com/cyberknights4911/robot2024/collect/CollectConstants.java @@ -22,7 +22,6 @@ @RecordBuilder public record CollectConstants( int motorCollectId, - int motorGuideId, int sensorId, int solenoidLeftId, int solenoidRightId, @@ -30,6 +29,4 @@ public record CollectConstants( double ejectTime, double ejectSpeed, double collectSpeed, - double feedShooterSpeed, - PidValues collectFeedBackValues, - PidValues guideFeedBackValues) {} + PidValues collectFeedBackValues) {} diff --git a/src/main/java/com/cyberknights4911/robot2024/collect/CollectIO.java b/src/main/java/com/cyberknights4911/robot2024/collect/CollectIO.java index 8d4271a..c9f36eb 100644 --- a/src/main/java/com/cyberknights4911/robot2024/collect/CollectIO.java +++ b/src/main/java/com/cyberknights4911/robot2024/collect/CollectIO.java @@ -41,17 +41,8 @@ public default void setCollectVelocity(double velocityRadPerSec) {} /** Stop collector in open loop. */ public default void stopCollector() {} - /** Run guide closed loop at the specified velocity. */ - public default void setGuideVelocity(double velocityRadPerSec) {} - - /** Stop guide in open loop. */ - public default void stopGuide() {} - public default void setCollecterPosition(boolean extended) {} - /** Set collect velocity PID constants. */ + /** Set velocity PID constants. */ public default void configureCollectPID(double kP, double kI, double kD) {} - - /** Set guide velocity PID constants. */ - public default void configureGuidePID(double kP, double kI, double kD) {} } diff --git a/src/main/java/com/cyberknights4911/robot2024/collect/CollectIOReal.java b/src/main/java/com/cyberknights4911/robot2024/collect/CollectIOReal.java index a5423fd..68a8e88 100644 --- a/src/main/java/com/cyberknights4911/robot2024/collect/CollectIOReal.java +++ b/src/main/java/com/cyberknights4911/robot2024/collect/CollectIOReal.java @@ -22,13 +22,8 @@ public class CollectIOReal implements CollectIO { private final CANSparkFlex collect; - private final CANSparkFlex guide; - private final RelativeEncoder collectEncoder; - private final RelativeEncoder guideEncoder; - private final SparkPIDController collectPidController; - private final SparkPIDController guidePidController; private final Solenoid left; private final Solenoid right; @@ -45,10 +40,6 @@ public CollectIOReal(CollectConstants constants, SparkBurnManager sparkBurnManag collectEncoder = collect.getEncoder(); collectPidController = collect.getPIDController(); - guide = new CANSparkFlex(constants.motorGuideId(), MotorType.kBrushless); - guideEncoder = guide.getEncoder(); - guidePidController = guide.getPIDController(); - left = new Solenoid(PneumaticsModuleType.REVPH, constants.solenoidLeftId()); right = new Solenoid(PneumaticsModuleType.REVPH, constants.solenoidRightId()); @@ -63,14 +54,9 @@ public void updateInputs(CollectIOInputs inputs) { Units.rotationsToRadians(collectEncoder.getPosition() / collectGearRatio); inputs.collectVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(collectEncoder.getVelocity() / collectGearRatio); - inputs.guidePositionRad = Units.rotationsToRadians(guideEncoder.getPosition()); - inputs.guideVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(guideEncoder.getVelocity()); inputs.collectAppliedVolts = collect.getAppliedOutput() * collect.getBusVoltage(); inputs.collectCurrentAmps = collect.getOutputCurrent(); - inputs.guideAppliedVolts = guide.getAppliedOutput() * guide.getBusVoltage(); - inputs.guideCurrentAmps = guide.getOutputCurrent(); inputs.leftSolenoid = left.get(); inputs.rightSolenoid = right.get(); @@ -95,17 +81,6 @@ public void stopCollector() { collect.stopMotor(); } - @Override - public void setGuideVelocity(double velocityRadPerSec) { - guidePidController.setReference( - Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec), ControlType.kVelocity); - } - - @Override - public void stopGuide() { - guide.stopMotor(); - } - @Override public void setCollecterPosition(boolean extended) { // TODO: actually extend or retract the solenoids @@ -119,38 +94,19 @@ public void configureCollectPID(double kP, double kI, double kD) { collectPidController.setFF(0, 0); } - @Override - public void configureGuidePID(double kP, double kI, double kD) { - guidePidController.setP(kP, 0); - guidePidController.setI(kI, 0); - guidePidController.setD(kD, 0); - guidePidController.setFF(0, 0); - } - private void configureDevices() { sparkBurnManager.maybeBurnConfig( () -> { SparkConfig.configNotLeader(collect); - SparkConfig.configNotLeader(guide); collect.setIdleMode(IdleMode.kBrake); - guide.setIdleMode(IdleMode.kBrake); - collect.setSmartCurrentLimit(40); - guide.setSmartCurrentLimit(40); - collect.enableVoltageCompensation(12.0); - guide.enableVoltageCompensation(12.0); collectEncoder.setPosition(0.0); collectEncoder.setMeasurementPeriod(10); collectEncoder.setAverageDepth(2); - - guideEncoder.setPosition(0.0); - guideEncoder.setMeasurementPeriod(10); - guideEncoder.setAverageDepth(2); }, - collect, - guide); + collect); } } diff --git a/src/main/java/com/cyberknights4911/robot2024/indexer/Indexer.java b/src/main/java/com/cyberknights4911/robot2024/indexer/Indexer.java new file mode 100644 index 0000000..6880066 --- /dev/null +++ b/src/main/java/com/cyberknights4911/robot2024/indexer/Indexer.java @@ -0,0 +1,92 @@ +// Copyright (c) 2024 FRC 4911 +// https://github.com/frc4911 +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package com.cyberknights4911.robot2024.indexer; + +import com.cyberknights4911.logging.LoggedTunableNumber; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public final class Indexer extends SubsystemBase { + private static final LoggedTunableNumber beamThreshold = + new LoggedTunableNumber("Indexer/BeamThreshold"); + private static final LoggedTunableNumber speed = new LoggedTunableNumber("Indexer/VelocityRPM"); + private static final LoggedTunableNumber kp = new LoggedTunableNumber("Indexer/Kp"); + private static final LoggedTunableNumber kd = new LoggedTunableNumber("Indexer/Kd"); + + private final IndexerIO indexerIO; + private final IndexerIOInputsAutoLogged inputs = new IndexerIOInputsAutoLogged(); + + private final Command runIndexAtTunableSpeed; + private final Command runIndexUntilSensor; + + public Indexer(IndexerConstants constants, IndexerIO indexerIO) { + super(); + this.indexerIO = indexerIO; + + speed.initDefault(constants.speed()); + kp.initDefault(constants.feedBackValues().kP()); + kd.initDefault(constants.feedBackValues().kP()); + + indexerIO.configurePID(kp.get(), 0.0, kd.get()); + + runIndexAtTunableSpeed = Commands.runOnce(() -> runVelocity(speed.get()), this); + + runIndexUntilSensor = + runIndexAtTunableSpeed.until(this::isBeamBreakBlocked).andThen(this::stop, this); + } + + @Override + public void periodic() { + indexerIO.updateInputs(inputs); + Logger.processInputs("Indexer", inputs); + + if (kp.hasChanged(hashCode()) || kd.hasChanged(hashCode())) { + indexerIO.configurePID(kp.get(), 0.0, kd.get()); + } + + if (DriverStation.isDisabled()) { + stop(); + } + } + + public boolean isBeamBreakBlocked() { + return inputs.beamBreakVoltage > beamThreshold.get(); + } + + /** Run closed loop at the specified velocity. */ + public void runVelocity(double velocityRpm) { + var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRpm); + indexerIO.setVelocity(velocityRadPerSec); + + Logger.recordOutput("Indexer/SetpointRPM", velocityRpm); + } + + public void stop() { + indexerIO.stop(); + } + + /** + * Returns a command that runs the collector at the tunable speed. This command completes + * immediately and the indexer will stay running afterward. + */ + public Command runIndexAtTunableSpeed() { + return runIndexAtTunableSpeed; + } + + /** + * Returns a command that runs the indexer at the tunable speed, but stops when the gamepiece + * reaches the sensor. + */ + public Command runIndexUntilSensor() { + return runIndexUntilSensor; + } +} diff --git a/src/main/java/com/cyberknights4911/robot2024/indexer/IndexerConstants.java b/src/main/java/com/cyberknights4911/robot2024/indexer/IndexerConstants.java new file mode 100644 index 0000000..ffbd77d --- /dev/null +++ b/src/main/java/com/cyberknights4911/robot2024/indexer/IndexerConstants.java @@ -0,0 +1,14 @@ +// Copyright (c) 2024 FRC 4911 +// https://github.com/frc4911 +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package com.cyberknights4911.robot2024.indexer; + +import com.cyberknights4911.util.PidValues; +import io.soabase.recordbuilder.core.RecordBuilder; + +@RecordBuilder +public record IndexerConstants(int motorId, int sensorId, double speed, PidValues feedBackValues) {} diff --git a/src/main/java/com/cyberknights4911/robot2024/indexer/IndexerIO.java b/src/main/java/com/cyberknights4911/robot2024/indexer/IndexerIO.java new file mode 100644 index 0000000..8b567da --- /dev/null +++ b/src/main/java/com/cyberknights4911/robot2024/indexer/IndexerIO.java @@ -0,0 +1,36 @@ +// Copyright (c) 2024 FRC 4911 +// https://github.com/frc4911 +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package com.cyberknights4911.robot2024.indexer; + +import org.littletonrobotics.junction.AutoLog; + +public interface IndexerIO { + + @AutoLog + public static class IndexerIOInputs { + public double positionRad = 0.0; + public double velocityRadPerSec = 0.0; + + public double appliedVolts = 0.0; + public double currentAmps = 0.0; + + public double beamBreakVoltage = 0.0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(IndexerIOInputs inputs) {} + + /** Run indexer closed loop at the specified velocity. */ + public default void setVelocity(double velocityRadPerSec) {} + + /** Stop indexer in open loop. */ + public default void stop() {} + + /** Set indexer velocity PID constants. */ + public default void configurePID(double kP, double kI, double kD) {} +} diff --git a/src/main/java/com/cyberknights4911/robot2024/indexer/IndexerIOInputsReal.java b/src/main/java/com/cyberknights4911/robot2024/indexer/IndexerIOInputsReal.java new file mode 100644 index 0000000..a0eec73 --- /dev/null +++ b/src/main/java/com/cyberknights4911/robot2024/indexer/IndexerIOInputsReal.java @@ -0,0 +1,86 @@ +// Copyright (c) 2024 FRC 4911 +// https://github.com/frc4911 +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package com.cyberknights4911.robot2024.indexer; + +import com.cyberknights4911.util.SparkBurnManager; +import com.cyberknights4911.util.SparkConfig; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.AnalogInput; + +public final class IndexerIOInputsReal implements IndexerIO { + private final CANSparkFlex motor; + private final RelativeEncoder encoder; + private final SparkPIDController pidController; + + private final AnalogInput beamBreak; + + private final SparkBurnManager sparkBurnManager; + + public IndexerIOInputsReal(IndexerConstants constants, SparkBurnManager sparkBurnManager) { + this.sparkBurnManager = sparkBurnManager; + + motor = new CANSparkFlex(constants.motorId(), MotorType.kBrushless); + encoder = motor.getEncoder(); + pidController = motor.getPIDController(); + + beamBreak = new AnalogInput(constants.sensorId()); + + configureDevices(); + } + + @Override + public void updateInputs(IndexerIOInputs inputs) { + inputs.positionRad = Units.rotationsToRadians(encoder.getPosition()); + inputs.velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity()); + inputs.appliedVolts = motor.getAppliedOutput() * motor.getBusVoltage(); + inputs.currentAmps = motor.getOutputCurrent(); + + inputs.beamBreakVoltage = beamBreak.getVoltage(); + } + + @Override + public void configurePID(double kP, double kI, double kD) { + pidController.setP(kP, 0); + pidController.setI(kI, 0); + pidController.setD(kD, 0); + pidController.setFF(0, 0); + } + + @Override + public void setVelocity(double velocityRadPerSec) { + pidController.setReference( + Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec), ControlType.kVelocity); + } + + @Override + public void stop() { + motor.stopMotor(); + } + + private void configureDevices() { + sparkBurnManager.maybeBurnConfig( + () -> { + SparkConfig.configNotLeader(motor); + + motor.setIdleMode(IdleMode.kBrake); + motor.setSmartCurrentLimit(40); + motor.enableVoltageCompensation(12.0); + + encoder.setPosition(0.0); + encoder.setMeasurementPeriod(10); + encoder.setAverageDepth(2); + }, + motor); + } +} diff --git a/src/main/java/com/cyberknights4911/robot2024/shooter/Shooter.java b/src/main/java/com/cyberknights4911/robot2024/shooter/Shooter.java index 62490a2..4f58386 100644 --- a/src/main/java/com/cyberknights4911/robot2024/shooter/Shooter.java +++ b/src/main/java/com/cyberknights4911/robot2024/shooter/Shooter.java @@ -138,13 +138,13 @@ public void setAimerPostion(double positionDegrees) { /** Run indexer closed loop at the specified velocity. */ public void runIndexerVelocity(double velocityRPM) { var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); - shooterIO.setIndexerVelocity(velocityRadPerSec); + shooterIO.setGuideVelocity(velocityRadPerSec); Logger.recordOutput("Shooter/IndexerSetpointRPM", velocityRPM); } public void stopIndexer() { - shooterIO.stopIndexer(); + shooterIO.stopGuide(); } public boolean isBeamBreakBlocked() { @@ -256,7 +256,7 @@ public Command setAimerAngle(LoggedTunableNumber angleDegrees) { Commands.waitUntil( () -> { return Math.abs( - Units.radiansToDegrees(inputs.indexerPositionRad) - angleDegrees.get()) + Units.radiansToDegrees(inputs.guidePositionRad) - angleDegrees.get()) < shooterPositionError.get(); })); } diff --git a/src/main/java/com/cyberknights4911/robot2024/shooter/ShooterConstants.java b/src/main/java/com/cyberknights4911/robot2024/shooter/ShooterConstants.java index 61ed63d..b8009e8 100644 --- a/src/main/java/com/cyberknights4911/robot2024/shooter/ShooterConstants.java +++ b/src/main/java/com/cyberknights4911/robot2024/shooter/ShooterConstants.java @@ -16,7 +16,7 @@ public record ShooterConstants( int shooterMotorTopId, int shooterMotorBottomId, int aimerMotorId, - int indexerMotorId, + int guideMotorId, int sensorId, int beamThreshold, double aimerGearRatio, diff --git a/src/main/java/com/cyberknights4911/robot2024/shooter/ShooterIO.java b/src/main/java/com/cyberknights4911/robot2024/shooter/ShooterIO.java index 32237b7..e51a856 100644 --- a/src/main/java/com/cyberknights4911/robot2024/shooter/ShooterIO.java +++ b/src/main/java/com/cyberknights4911/robot2024/shooter/ShooterIO.java @@ -18,8 +18,8 @@ public static class ShooterIOInputs { public double shooterBottomVelocityRadPerSec = 0.0; public double aimerPositionRad = 0.0; public double aimerVelocityRadPerSec = 0.0; - public double indexerPositionRad = 0.0; - public double indexerVelocityRadPerSec = 0.0; + public double guidePositionRad = 0.0; + public double guideVelocityRadPerSec = 0.0; public double shooterTopAppliedVolts = 0.0; public double shooterTopCurrentAmps = 0.0; @@ -27,8 +27,8 @@ public static class ShooterIOInputs { public double shooterBottomCurrentAmps = 0.0; public double aimerAppliedVolts = 0.0; public double aimerCurrentAmps = 0.0; - public double indexerAppliedVolts = 0.0; - public double indexerCurrentAmps = 0.0; + public double guideAppliedVolts = 0.0; + public double guideCurrentAmps = 0.0; public double beamBreakValue = 0.0; } @@ -48,14 +48,14 @@ public default void stopShooter() {} /** Run closed loop to the specified position. */ public default void setAimerPosition(double position, double ffVolts) {} - /** Run indexer in closed loop at the specified velocity. */ - public default void setIndexerVelocity(double velocityRadPerSec) {} + /** Run guide in closed loop at the specified velocity. */ + public default void setGuideVelocity(double velocityRadPerSec) {} /** Stop aimer in open loop. */ public default void stopAimer() {} - /** Stop indexer in open loop. */ - public default void stopIndexer() {} + /** Stop guide in open loop. */ + public default void stopGuide() {} /** Set velocity PID constants. */ public default void configureShooterPID(double kP, double kI, double kD) {} diff --git a/src/main/java/com/cyberknights4911/robot2024/shooter/ShooterIOReal.java b/src/main/java/com/cyberknights4911/robot2024/shooter/ShooterIOReal.java index 169ba63..081659d 100644 --- a/src/main/java/com/cyberknights4911/robot2024/shooter/ShooterIOReal.java +++ b/src/main/java/com/cyberknights4911/robot2024/shooter/ShooterIOReal.java @@ -24,16 +24,16 @@ public class ShooterIOReal implements ShooterIO { private final CANSparkFlex shooterTop; private final CANSparkFlex shooterBottom; private final CANSparkFlex aimer; - private final CANSparkFlex indexer; + private final CANSparkFlex guide; private final RelativeEncoder shooterTopEncoder; private final RelativeEncoder shooterBottomEncoder; private final RelativeEncoder aimerEncoder; - private final RelativeEncoder indexerEncoder; + private final RelativeEncoder guideEncoder; private final SparkPIDController shooterPidController; private final SparkPIDController aimerPidController; - private final SparkPIDController indexerPidController; + private final SparkPIDController guidePidController; private final AnalogInput beamBreak; @@ -55,10 +55,9 @@ public ShooterIOReal(ShooterConstants constants, SparkBurnManager sparkBurnManag aimerEncoder = aimer.getEncoder(); aimerPidController = aimer.getPIDController(); - // motor for the indexer and moving notes to the shooter to shoot - indexer = new CANSparkFlex(constants.indexerMotorId(), MotorType.kBrushless); - indexerEncoder = indexer.getEncoder(); - indexerPidController = indexer.getPIDController(); + guide = new CANSparkFlex(constants.guideMotorId(), MotorType.kBrushless); + guideEncoder = guide.getEncoder(); + guidePidController = guide.getPIDController(); beamBreak = new AnalogInput(constants.sensorId()); @@ -91,8 +90,8 @@ public void setAimerPosition(double positionRadians, double ffVolts) { } @Override - public void setIndexerVelocity(double velocityRadiansPerSecond) { - indexerPidController.setReference( + public void setGuideVelocity(double velocityRadiansPerSecond) { + guidePidController.setReference( Units.radiansPerSecondToRotationsPerMinute(velocityRadiansPerSecond), ControlType.kVelocity); } @@ -108,9 +107,9 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.aimerPositionRad = Units.rotationsToRadians(aimerEncoder.getPosition()) / aimerGearRatio; inputs.aimerVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(aimerEncoder.getVelocity()) / aimerGearRatio; - inputs.indexerPositionRad = Units.rotationsToRadians(indexerEncoder.getPosition()); - inputs.indexerVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(indexerEncoder.getVelocity()); + inputs.guidePositionRad = Units.rotationsToRadians(guideEncoder.getPosition()); + inputs.guideVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(guideEncoder.getVelocity()); inputs.shooterTopAppliedVolts = shooterTop.getAppliedOutput() * shooterTop.getBusVoltage(); inputs.shooterTopCurrentAmps = shooterTop.getOutputCurrent(); @@ -119,8 +118,8 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.shooterBottomCurrentAmps = shooterBottom.getOutputCurrent(); inputs.aimerAppliedVolts = aimer.getAppliedOutput() * aimer.getBusVoltage(); inputs.aimerCurrentAmps = aimer.getOutputCurrent(); - inputs.indexerAppliedVolts = indexer.getAppliedOutput() * indexer.getBusVoltage(); - inputs.indexerCurrentAmps = indexer.getOutputCurrent(); + inputs.guideAppliedVolts = guide.getAppliedOutput() * guide.getBusVoltage(); + inputs.guideCurrentAmps = guide.getOutputCurrent(); inputs.beamBreakValue = beamBreak.getVoltage(); } @@ -136,8 +135,8 @@ public void stopAimer() { } @Override - public void stopIndexer() { - indexer.stopMotor(); + public void stopGuide() { + guide.stopMotor(); } @Override @@ -162,23 +161,23 @@ private void configureDevices() { SparkConfig.configLeaderFollower(shooterBottom); SparkConfig.configLeaderFollower(shooterTop); SparkConfig.configNotLeader(aimer); - SparkConfig.configNotLeader(indexer); + SparkConfig.configNotLeader(guide); shooterBottom.follow(shooterTop, true); shooterTop.setSmartCurrentLimit(40); shooterBottom.setSmartCurrentLimit(40); aimer.setSmartCurrentLimit(40); - indexer.setSmartCurrentLimit(30); + guide.setSmartCurrentLimit(30); shooterTop.enableVoltageCompensation(12); shooterBottom.enableVoltageCompensation(12); aimer.enableVoltageCompensation(12); - indexer.enableVoltageCompensation(12); + guide.enableVoltageCompensation(12); shooterTop.setIdleMode(IdleMode.kCoast); shooterBottom.setIdleMode(IdleMode.kCoast); aimer.setIdleMode(IdleMode.kBrake); - indexer.setIdleMode(IdleMode.kBrake); + guide.setIdleMode(IdleMode.kBrake); shooterTopEncoder.setPosition(0.0); shooterTopEncoder.setMeasurementPeriod(10); @@ -189,13 +188,13 @@ private void configureDevices() { aimerEncoder.setPosition(0.0); aimerEncoder.setMeasurementPeriod(10); aimerEncoder.setAverageDepth(2); - indexerEncoder.setPosition(0.0); - indexerEncoder.setMeasurementPeriod(10); - indexerEncoder.setAverageDepth(2); + guideEncoder.setPosition(0.0); + guideEncoder.setMeasurementPeriod(10); + guideEncoder.setAverageDepth(2); }, shooterTop, shooterBottom, aimer, - indexer); + guide); } }