diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 2da709d..fb5cbb5 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -4,7 +4,6 @@ package frc.robot.subsystems; -import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -19,52 +18,44 @@ import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.controls.TorqueCurrentFOC; - - public class ArmSubsystem extends SubsystemBase { private TalonFX armMotor, rollerMotor; - + private final DutyCycleOut m_dutyCycleRequest = new DutyCycleOut(0.0); private final VoltageOut m_voltageRequest = new VoltageOut(0.0); private final TorqueCurrentFOC m_torqueRequest = new TorqueCurrentFOC(0.0); public ArmSubsystem() { - armMotor = new TalonFX(Constants.Arm.ROLLER_MOTOR_ID); - rollerMotor = new TalonFX(Constants.Arm.ARM_MOTOR_ID); - - CurrentLimitsConfigs rollerClc = - new CurrentLimitsConfigs() - .withStatorCurrentLimit(50) - .withStatorCurrentLimitEnable(false) - .withSupplyCurrentLimit(30) - .withSupplyCurrentLimitEnable(false); - - CurrentLimitsConfigs armClc = - new CurrentLimitsConfigs() + armMotor = new TalonFX(Constants.Arm.ARM_MOTOR_ID); + rollerMotor = new TalonFX(Constants.Arm.ROLLER_MOTOR_ID); + + CurrentLimitsConfigs rollerClc = new CurrentLimitsConfigs() + .withStatorCurrentLimit(50) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(30) + .withSupplyCurrentLimitEnable(true); + + CurrentLimitsConfigs armClc = new CurrentLimitsConfigs() .withStatorCurrentLimit(100) .withStatorCurrentLimitEnable(true) .withSupplyCurrentLimit(50) .withSupplyCurrentLimitEnable(true); - MotorOutputConfigs rollerMotorOutputConfigs = - new MotorOutputConfigs() + MotorOutputConfigs rollerMotorOutputConfigs = new MotorOutputConfigs() .withInverted(InvertedValue.CounterClockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake); + .withNeutralMode(NeutralModeValue.Coast); - MotorOutputConfigs armMotorOutputConfigs = - new MotorOutputConfigs() + MotorOutputConfigs armMotorOutputConfigs = new MotorOutputConfigs() .withInverted(InvertedValue.CounterClockwise_Positive) .withNeutralMode(NeutralModeValue.Brake); - TalonFXConfiguration rollerConfig = - new TalonFXConfiguration() + TalonFXConfiguration rollerConfig = new TalonFXConfiguration() .withCurrentLimits(rollerClc) .withMotorOutput(rollerMotorOutputConfigs); - TalonFXConfiguration armConfig = - new TalonFXConfiguration() - .withCurrentLimits(rollerClc) - .withMotorOutput(rollerMotorOutputConfigs); + TalonFXConfiguration armConfig = new TalonFXConfiguration() + .withCurrentLimits(armClc) + .withMotorOutput(armMotorOutputConfigs); TalonFXConfigurator rollerConfigurator = rollerMotor.getConfigurator(); TalonFXConfigurator armConfigurator = armMotor.getConfigurator(); @@ -74,19 +65,18 @@ public ArmSubsystem() { } - public void setRollerDutyCycle(double voltage) { - rollerMotor.setControl(m_voltageRequest.withOutput(voltage)); + public void setRollerDutyCycle(double percent) { + rollerMotor.setControl(m_dutyCycleRequest.withOutput(percent)); } public void setRollerVoltage(double voltage) { - rollerMotor.setControl(m_dutyCycleRequest.withOutput(voltage)); + rollerMotor.setControl(m_voltageRequest.withOutput(voltage)); } public void setArmTorqueCurrent(double current) { armMotor.setControl(m_torqueRequest.withOutput(current)); } - @Override public void periodic() { // This method will be called once per scheduler run