FairportRobotics
/
Castor2026
mirror of https://github.com/FairportRobotics/Castor2026.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Kicker PID

This commit is contained in:
Tyler Wilcox 2026-04-04 15:25:52 -04:00
parent 317d21d000
commit 57fa16b4b3
1 changed files with 51 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
src/main/java/frc/robot/subsystems/HopperSubsystem.java
View File

@ -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<AngularVelocity> 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();