diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 12791edd..436c3487 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -157,9 +157,9 @@ private void configureBindings() { new Trigger(copilot::getXButton).whileTrue(hood.retractCommand().withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); new Trigger(copilot::getLeftBumperButton).whileTrue(indexer.indexCommand(-IndexerConstants.SPINDEXDER_POWER, - -IndexerConstants.TRANSFER_POWER)); + -IndexerConstants.TRANSFER_VELOCITY)); new Trigger(copilot::getRightBumperButton).whileTrue(indexer.indexCommand(IndexerConstants.SPINDEXDER_POWER, - IndexerConstants.TRANSFER_POWER)); + IndexerConstants.TRANSFER_VELOCITY)); new Trigger(copilot::getStartButton).onTrue(collector.stowPivotCommand()); @@ -193,8 +193,7 @@ private void configureBindings() { // new Trigger(() -> DriverStation.isEnabled()).onTrue(hood.zeroCommand()); - new Trigger(copilot::getAButton).whileTrue(indexer.autoIndex(IndexerConstants.SPINDEXDER_POWER, IndexerConstants.TRANSFER_POWER)).onFalse(new InstantCommand(() -> indexer.stop())); - + new Trigger(copilot::getAButton).whileTrue(indexer.autoIndex(IndexerConstants.SPINDEXDER_POWER, IndexerConstants.TRANSFER_VELOCITY)).onFalse(new InstantCommand(() -> indexer.stop())); new Trigger(copilot::getYButton).whileTrue(indexer.indexCommand(-0.5).withTimeout(0.1).andThen(indexer.indexCommand(1))); diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index 39e9f714..fd70c20f 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -8,6 +8,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.sim.TalonFXSimState; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; @@ -58,10 +59,15 @@ public class IndexerConstants { public static final Current TRANSFER_MOTOR_STATOR_LIMIT = Amps.of(40); // temp public static final boolean TRANSFER_MOTOR_BRAKE_MODE = true; // temp - public static final double TRANSFER_POWER = 0.8; + public static final double TRANSFER_POWER = 0.7; + public static final double TRANSFER_VELOCITY = 70; public static final Current TRANSFER_SUPPLY_LIMIT = Amps.of(40); // temp public static final boolean TRANSFER_SUPPLY_LIMIT_ENABLE = true; // temp + public static final double TRANSFER_kP = 0.2; // temp + public static final double TRANSFER_kV = 0.124; // temp + public static final double TRANSFER_kS = 0.25; + // sim public static final AngularVelocity SIM_INDEX_THRESHOLD = RotationsPerSecond.of(1); // temp } @@ -72,6 +78,8 @@ public class IndexerConstants { private final DutyCycleOut spindexerDutyCycle; private final DutyCycleOut transferDutyCycle; + private VelocityVoltage transferVelocityVoltage; + private LinearSystemSim spindexerSim; private TalonFXSimState motorSim; @@ -88,12 +96,17 @@ public Indexer() { spindexerDutyCycle = new DutyCycleOut(0d); transferDutyCycle = new DutyCycleOut(0d); + VelocityVoltage transferVelocityVoltage = new VelocityVoltage(0); TalonFXConfiguration spindexerConfig = spindexerMotor.getConfig(); TalonFXConfiguration transferConfig = transferMotor.getConfig(); spindexerConfig.CurrentLimits.SupplyCurrentLimitEnable = IndexerConstants.SPINDEXER_SUPPLY_LIMIT_ENABLE; spindexerConfig.CurrentLimits.SupplyCurrentLimit = IndexerConstants.SPINDEXER_SUPPLY_LIMIT.in(Amps); + + transferConfig.Slot0.kP = IndexerConstants.TRANSFER_kP; + transferConfig.Slot0.kV = IndexerConstants.TRANSFER_kV; + transferConfig.CurrentLimits.SupplyCurrentLimitEnable = IndexerConstants.TRANSFER_SUPPLY_LIMIT_ENABLE; transferConfig.CurrentLimits.SupplyCurrentLimit = IndexerConstants.TRANSFER_SUPPLY_LIMIT.in(Amps); @@ -178,6 +191,10 @@ public void setTransferPower(double power) { transferMotor.setControl(transferDutyCycle.withOutput(power)); } + public void setTransferVelocity(double velocity) { + transferMotor.setControl(transferVelocityVoltage.withVelocity(velocity)); + } + /** * stops all movement to the transfer motor */ @@ -191,12 +208,12 @@ public void stopTransfer() { */ public void setPower(double power) { setSpindexerPower(power); - setTransferPower(power); + setTransferVelocity(power); } public void setPower(double spindexerPower, double transferPower) { setSpindexerPower(spindexerPower); - setTransferPower(transferPower); + setTransferVelocity(transferPower); } /** @@ -217,25 +234,25 @@ public Command indexCommand(double power) { return new StartEndCommand(() -> setPower(power), () -> stop(), this); } - public Command indexCommand(DoubleSupplier spindexerPower, DoubleSupplier transferPower) { - return new StartEndCommand(() -> setPower(spindexerPower.getAsDouble(), transferPower.getAsDouble()), this::stop); + public Command indexCommand(DoubleSupplier spindexerPower, DoubleSupplier transferVelocity) { + return new StartEndCommand(() -> setPower(spindexerPower.getAsDouble(), transferVelocity.getAsDouble()), this::stop); } - public Command autoIndex(DoubleSupplier spindexerPower, DoubleSupplier transferPower) { - return new InstantCommand(() -> setTransferPower(transferPower.getAsDouble())) + public Command autoIndex(DoubleSupplier spindexerPower, DoubleSupplier transferVelocity) { + return new InstantCommand(() -> setTransferVelocity(transferVelocity.getAsDouble())) .andThen(new WaitCommand(IndexerConstants.SPINDEXER_DELAY)) - .andThen(indexCommand(spindexerPower, transferPower)); + .andThen(indexCommand(spindexerPower, transferVelocity)); } - public Command autoIndex(double spindexerPower, double transferPower) { - return autoIndex(() -> spindexerPower, () -> transferPower); + public Command autoIndex(double spindexerPower, double transferVelocity) { + return autoIndex(() -> spindexerPower, () -> transferVelocity); } - public Command indexCommand(double spindexerPower, double transferPower) { - return indexCommand(() -> spindexerPower, () -> transferPower); + public Command indexCommand(double spindexerPower, double transferVelocity) { + return indexCommand(() -> spindexerPower, () -> transferVelocity); } - public Command transferCommand(DoubleSupplier transferPower) { - return new RunCommand(() -> setTransferPower(transferPower.getAsDouble())); + public Command transferCommand(DoubleSupplier transferVelocity) { + return new RunCommand(() -> setTransferVelocity(transferVelocity.getAsDouble())); } }