61 lines
1.7 KiB
Java
61 lines
1.7 KiB
Java
package frc.robot.subsystems;
|
|
|
|
import java.util.function.DoubleSupplier;
|
|
|
|
import org.littletonrobotics.junction.Logger;
|
|
|
|
import com.revrobotics.AbsoluteEncoder;
|
|
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.HoodConstants;
|
|
|
|
public class Hood extends SubsystemBase {
|
|
private SparkMax motor;
|
|
|
|
private AbsoluteEncoder encoder;
|
|
|
|
private SparkClosedLoopController controller;
|
|
|
|
private double currentTargetRadians;
|
|
|
|
public Hood() {
|
|
motor = new SparkMax(HoodConstants.kMotorCANID, MotorType.kBrushless);
|
|
|
|
encoder = motor.getAbsoluteEncoder();
|
|
|
|
controller = motor.getClosedLoopController();
|
|
|
|
currentTargetRadians = HoodConstants.kStartupAngle;
|
|
}
|
|
|
|
@Override
|
|
public void periodic() {
|
|
Logger.recordOutput("Hood/CurrentTarget", currentTargetRadians);
|
|
Logger.recordOutput("Hood/CurrentAngle", encoder.getPosition());
|
|
Logger.recordOutput("Hood/AtSetpoint", controller.isAtSetpoint());
|
|
}
|
|
|
|
public Command trackToAngle(DoubleSupplier radianAngleSupplier) {
|
|
return run(() -> {
|
|
currentTargetRadians = radianAngleSupplier.getAsDouble();
|
|
|
|
controller.setSetpoint(currentTargetRadians, ControlType.kPosition);
|
|
});
|
|
}
|
|
|
|
public Command stop() {
|
|
return run(() -> {
|
|
motor.disable();
|
|
});
|
|
}
|
|
|
|
public double getTargetRadians() {
|
|
return currentTargetRadians;
|
|
}
|
|
}
|