diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java index 6f0f1d5..e8e4e53 100644 --- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java @@ -6,32 +6,72 @@ package frc.robot.subsystems; import org.fairportrobotics.frc.posty.TestableSubsystem; import org.fairportrobotics.frc.posty.test.PostTest; +import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.units.AngularVelocityUnit; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; + import static org.fairportrobotics.frc.posty.assertions.Assertions.*; import frc.robot.Constants; public class HopperSubsystem extends TestableSubsystem { private TalonFX spindexerMotor; + + private TalonFX kickerMotor; + private VelocityVoltage kickerControl = new VelocityVoltage(0); + private StatusSignal kickerVelocity; + /** Creates a new HopperSubsystem. */ public HopperSubsystem() { spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID); - spindexerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION)); - // spindexerMotor.getConfigurator().apply(new ClosedLoopGeneralConfigs() - // .); + spindexerMotor.getConfigurator().apply( + new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION)) + .withSlot0( + new Slot0Configs() + .withKP(0) + .withKI(0) + .withKD(0) + .withKS(0))); kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID); - kickerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION)); + kickerMotor.getConfigurator().apply( + new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION) + ) + .withSlot0(new Slot0Configs() + .withKP(0) + .withKI(0) + .withKD(0) + .withKS(0) + )); + kickerVelocity = kickerMotor.getVelocity(); } - public void feedKicker() {kickerMotor.set(0.75);} + public void feedKicker() { + kickerMotor.setControl(kickerControl.withVelocity(1000)); + // kickerMotor.set(0.75); + } - public void reverseKicker() {kickerMotor.set(-0.5);} + public void reverseKicker() { + kickerMotor.setControl(kickerControl.withVelocity(-500)); + // kickerMotor.set(-0.5); + } public void stopKicker() {kickerMotor.stopMotor();} @@ -39,6 +79,11 @@ public class HopperSubsystem extends TestableSubsystem { public void spindexerOff() { spindexerMotor.stopMotor();} + @Override + public void periodic() { + Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond)); + } + @PostTest(enabled = true) public void hopperSystem_CANDevicesConnected(){ assertThat(kickerMotor.isConnected()).as("Kicker motor not connected!").isTrue();