89 lines
2.8 KiB
Java
89 lines
2.8 KiB
Java
package frc.robot.subsystems;
|
|
|
|
import java.util.Optional;
|
|
import java.util.function.DoubleSupplier;
|
|
|
|
import org.littletonrobotics.junction.Logger;
|
|
|
|
import com.revrobotics.AbsoluteEncoder;
|
|
import com.revrobotics.PersistMode;
|
|
import com.revrobotics.ResetMode;
|
|
import com.revrobotics.spark.SparkClosedLoopController;
|
|
import com.revrobotics.spark.SparkMax;
|
|
import com.revrobotics.spark.SparkBase.ControlType;
|
|
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
|
|
|
import edu.wpi.first.wpilibj2.command.Command;
|
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
import frc.robot.constants.IntakePivotConstants;
|
|
import frc.robot.constants.IntakePivotConstants.IntakePivotPosition;
|
|
|
|
public class IntakePivot extends SubsystemBase {
|
|
private SparkMax leftMotor;
|
|
private SparkMax rightMotor;
|
|
|
|
private AbsoluteEncoder encoder;
|
|
|
|
private SparkClosedLoopController controller;
|
|
|
|
private IntakePivotPosition currentTargetPosition;
|
|
|
|
public IntakePivot() {
|
|
leftMotor = new SparkMax(IntakePivotConstants.kLeftMotorCANID, MotorType.kBrushless);
|
|
rightMotor = new SparkMax(IntakePivotConstants.kRightMotorCANID, MotorType.kBrushless);
|
|
|
|
leftMotor.configure(
|
|
IntakePivotConstants.KLeftMotorConfig,
|
|
ResetMode.kResetSafeParameters,
|
|
PersistMode.kPersistParameters
|
|
);
|
|
|
|
rightMotor.configure(
|
|
IntakePivotConstants.kRightMotorConfig,
|
|
ResetMode.kResetSafeParameters,
|
|
PersistMode.kPersistParameters
|
|
);
|
|
|
|
controller = leftMotor.getClosedLoopController();
|
|
|
|
encoder = leftMotor.getAbsoluteEncoder();
|
|
}
|
|
|
|
@Override
|
|
public void periodic() {
|
|
Logger.recordOutput(
|
|
"IntakePivot/TargetPosition",
|
|
currentTargetPosition == null ? -1 : currentTargetPosition.getPositionRadians());
|
|
Logger.recordOutput("IntakePivot/CurrentPosition", encoder.getPosition());
|
|
Logger.recordOutput("IntakePivot/AtSetpoint", controller.isAtSetpoint());
|
|
}
|
|
|
|
public Command maintainPosition(IntakePivotPosition position) {
|
|
currentTargetPosition = position;
|
|
|
|
return run(() -> {
|
|
if(currentTargetPosition == null) {
|
|
leftMotor.disable();
|
|
} else {
|
|
controller.setSetpoint(currentTargetPosition.getPositionRadians(), ControlType.kPosition);
|
|
}
|
|
});
|
|
}
|
|
|
|
public Command manualSpeed(DoubleSupplier speed) {
|
|
currentTargetPosition = null;
|
|
|
|
return run(() -> {
|
|
leftMotor.set(speed.getAsDouble() * IntakePivotConstants.kMaxManualSpeedMultiplier);
|
|
});
|
|
}
|
|
|
|
public Command stop() {
|
|
return manualSpeed(() -> 0);
|
|
}
|
|
|
|
public Optional<IntakePivotPosition> getCurrentTargetPosition() {
|
|
return Optional.ofNullable(currentTargetPosition);
|
|
}
|
|
}
|