127 lines
3.6 KiB
Java
127 lines
3.6 KiB
Java
package first.robot.subsystems;
|
|
|
|
import java.util.Optional;
|
|
import java.util.function.DoubleSupplier;
|
|
|
|
import org.wpilib.command2.Command;
|
|
import org.wpilib.command2.SubsystemBase;
|
|
import org.wpilib.epilogue.Logged;
|
|
import org.wpilib.math.geometry.Pose2d;
|
|
import org.wpilib.math.interpolation.InterpolatingDoubleTreeMap;
|
|
|
|
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.HoodConstants;
|
|
import first.robot.constants.ShooterConstants.ShooterSpeeds;
|
|
import first.robot.utilities.Utilities;
|
|
|
|
@Logged
|
|
public class Hood extends SubsystemBase {
|
|
private SparkMax motor;
|
|
|
|
private RelativeEncoder encoder;
|
|
|
|
private SparkClosedLoopController controller;
|
|
|
|
private double currentTargetRadians;
|
|
|
|
public Hood() {
|
|
motor = new SparkMax(
|
|
HoodConstants.kCANPort,
|
|
HoodConstants.kMotorCANID,
|
|
MotorType.kBrushless
|
|
);
|
|
|
|
motor.configure(
|
|
HoodConstants.kConfig,
|
|
ResetMode.kResetSafeParameters,
|
|
PersistMode.kPersistParameters
|
|
);
|
|
|
|
encoder = motor.getEncoder();
|
|
encoder.setPosition(HoodConstants.kStartupAngle);
|
|
|
|
controller = motor.getClosedLoopController();
|
|
|
|
currentTargetRadians = HoodConstants.kStartupAngle;
|
|
}
|
|
|
|
public double getHoodTargetDegrees() {
|
|
return Math.toDegrees(currentTargetRadians);
|
|
}
|
|
|
|
public double getHoodTargetRadians() {
|
|
return currentTargetRadians;
|
|
}
|
|
|
|
public double getCurrentHoodAngleDegrees() {
|
|
return Math.toDegrees(encoder.getPosition().get());
|
|
}
|
|
|
|
public double getCurrentHoodAngleRadians() {
|
|
return encoder.getPosition().get();
|
|
}
|
|
|
|
public double getOutputVoltage() {
|
|
return motor.getAppliedOutput().get() * motor.getBusVoltage().get();
|
|
}
|
|
|
|
public double getOutputCurrent() {
|
|
return motor.getOutputCurrent().get();
|
|
}
|
|
|
|
public boolean isHoodAtSetpoint() {
|
|
return controller.isAtSetpoint().get();
|
|
}
|
|
|
|
public Command trackToAnglePoseBased(Drivetrain drivetrain, Shooter shooter) {
|
|
return trackToAngle(() -> {
|
|
Pose2d drivetrainPose = drivetrain.getPose();
|
|
Pose2d hubPose = Utilities.getHubPose();
|
|
|
|
double distance = drivetrainPose.getTranslation()
|
|
.getDistance(hubPose.getTranslation());
|
|
|
|
Optional<ShooterSpeeds> currentSpeeds = shooter.getTargetSpeeds();
|
|
|
|
if(currentSpeeds.isPresent()) {
|
|
InterpolatingDoubleTreeMap map = HoodConstants.kHoodInterpolators.get(currentSpeeds.get());
|
|
|
|
if(map != null) {
|
|
return Math.clamp(map.get(distance), 0, 40);
|
|
} else {
|
|
return 0;
|
|
}
|
|
} else {
|
|
return 0;
|
|
}
|
|
});
|
|
}
|
|
|
|
public Command trackToAngle(DoubleSupplier radianAngleSupplier) {
|
|
return run(() -> {
|
|
currentTargetRadians = radianAngleSupplier.getAsDouble();
|
|
|
|
controller.setSetpoint(currentTargetRadians, ControlType.kPosition);
|
|
});
|
|
}
|
|
|
|
public Command manualSpeed(DoubleSupplier speed) {
|
|
currentTargetRadians = 0;
|
|
|
|
return run(() -> {
|
|
motor.setThrottle(speed.getAsDouble() * HoodConstants.kMaxManualSpeedMultiplier);
|
|
});
|
|
}
|
|
|
|
public Command stop() {
|
|
return manualSpeed(() -> 0);
|
|
}
|
|
}
|