89 lines
2.2 KiB
Java
89 lines
2.2 KiB
Java
package first.robot.subsystems;
|
|
|
|
import java.util.function.DoubleSupplier;
|
|
|
|
import org.wpilib.command2.Command;
|
|
import org.wpilib.command2.SubsystemBase;
|
|
import org.wpilib.epilogue.Logged;
|
|
|
|
import com.revrobotics.PersistMode;
|
|
import com.revrobotics.RelativeEncoder;
|
|
import com.revrobotics.ResetMode;
|
|
import com.revrobotics.spark.SparkClosedLoopController;
|
|
import com.revrobotics.spark.SparkLowLevel.ControlType;
|
|
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
|
import com.revrobotics.spark.SparkMax;
|
|
|
|
import first.robot.constants.ClimberConstants;
|
|
import first.robot.constants.ClimberConstants.ClimberPositions;
|
|
|
|
@Logged
|
|
public class Climber extends SubsystemBase {
|
|
private SparkMax motor;
|
|
|
|
private RelativeEncoder encoder;
|
|
|
|
private SparkClosedLoopController controller;
|
|
|
|
private ClimberPositions targetPosition;
|
|
|
|
public Climber() {
|
|
motor = new SparkMax(
|
|
ClimberConstants.kCANPort,
|
|
ClimberConstants.kMotorCANID,
|
|
MotorType.kBrushless
|
|
);
|
|
|
|
motor.configure(
|
|
ClimberConstants.kMotorConfig,
|
|
ResetMode.kResetSafeParameters,
|
|
PersistMode.kPersistParameters
|
|
);
|
|
|
|
encoder = motor.getEncoder();
|
|
|
|
controller = motor.getClosedLoopController();
|
|
|
|
targetPosition = null;
|
|
}
|
|
|
|
public double getTargetPositionMeters() {
|
|
if (targetPosition == null) {
|
|
return -1;
|
|
} else {
|
|
return targetPosition.getPositionMeters();
|
|
}
|
|
}
|
|
|
|
public double getCurrentPositionMeters() {
|
|
return encoder.getPosition().get();
|
|
}
|
|
|
|
public boolean isAtSetpoint() {
|
|
return controller.isAtSetpoint().get();
|
|
}
|
|
|
|
public Command maintainPosition(ClimberPositions position) {
|
|
return run(() -> {
|
|
targetPosition = position;
|
|
|
|
controller.setSetpoint(
|
|
position.getPositionMeters(),
|
|
ControlType.kPosition
|
|
);
|
|
});
|
|
}
|
|
|
|
public Command manualSpeed(DoubleSupplier speed) {
|
|
return run(() -> {
|
|
targetPosition = null;
|
|
|
|
motor.setThrottle(speed.getAsDouble());
|
|
});
|
|
}
|
|
|
|
public Command stop() {
|
|
return manualSpeed(() -> 0);
|
|
}
|
|
}
|