100 lines
3.0 KiB
Java
100 lines
3.0 KiB
Java
package frc.robot.subsystems;
|
|
|
|
import java.util.function.DoubleSupplier;
|
|
|
|
import org.littletonrobotics.junction.Logger;
|
|
|
|
import com.revrobotics.AbsoluteEncoder;
|
|
import com.revrobotics.PersistMode;
|
|
import com.revrobotics.RelativeEncoder;
|
|
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.wpilibj.Timer;
|
|
import edu.wpi.first.wpilibj2.command.Command;
|
|
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
|
import frc.robot.constants.HoodConstants;
|
|
|
|
public class Hood extends SubsystemBase {
|
|
private SparkMax motor;
|
|
|
|
private RelativeEncoder encoder;
|
|
|
|
private SparkClosedLoopController controller;
|
|
|
|
private Trigger resetTrigger;
|
|
private Trigger timerTrigger;
|
|
|
|
private Timer resetTimer;
|
|
|
|
private double currentTargetDegrees;
|
|
|
|
public Hood() {
|
|
motor = new SparkMax(HoodConstants.kMotorCANID, MotorType.kBrushless);
|
|
|
|
motor.configure(
|
|
HoodConstants.kConfig,
|
|
ResetMode.kResetSafeParameters,
|
|
PersistMode.kPersistParameters
|
|
);
|
|
|
|
encoder = motor.getEncoder();
|
|
|
|
controller = motor.getClosedLoopController();
|
|
|
|
resetTimer = new Timer();
|
|
resetTimer.reset();
|
|
|
|
resetTrigger = new Trigger(() -> (motor.getOutputCurrent() > HoodConstants.kAmpsToTriggerPositionReset));
|
|
resetTrigger.onTrue(new InstantCommand(resetTimer::start));
|
|
resetTrigger.onFalse(new InstantCommand(() -> {
|
|
resetTimer.stop();
|
|
resetTimer.reset();
|
|
}));
|
|
|
|
timerTrigger = new Trigger(() -> resetTimer.hasElapsed(HoodConstants.kTimeAboveThresholdToReset));
|
|
timerTrigger.onTrue(new InstantCommand(() -> {
|
|
encoder.setPosition(0);
|
|
resetTimer.reset();
|
|
}));
|
|
|
|
currentTargetDegrees = HoodConstants.kStartupAngle;
|
|
}
|
|
|
|
@Override
|
|
public void periodic() {
|
|
Logger.recordOutput("Hood/CurrentTarget", currentTargetDegrees);
|
|
Logger.recordOutput("Hood/CurrentAngle", encoder.getPosition());
|
|
Logger.recordOutput("Hood/AtSetpoint", controller.isAtSetpoint());
|
|
}
|
|
|
|
public Command trackToAngle(DoubleSupplier degreeAngleSupplier) {
|
|
return run(() -> {
|
|
currentTargetDegrees = degreeAngleSupplier.getAsDouble();
|
|
|
|
controller.setSetpoint(currentTargetDegrees, ControlType.kPosition);
|
|
});
|
|
}
|
|
|
|
public Command manualSpeed(DoubleSupplier speed) {
|
|
currentTargetDegrees = 0;
|
|
|
|
return run(() -> {
|
|
motor.set(speed.getAsDouble() * HoodConstants.kMaxManualSpeedMultiplier);
|
|
});
|
|
}
|
|
|
|
public Command stop() {
|
|
return manualSpeed(() -> 0);
|
|
}
|
|
|
|
public double getTargetDegrees() {
|
|
return currentTargetDegrees;
|
|
}
|
|
}
|