diff --git a/src/main/java/org/team340/robot/Constants.java b/src/main/java/org/team340/robot/Constants.java index 2c98091..0170737 100644 --- a/src/main/java/org/team340/robot/Constants.java +++ b/src/main/java/org/team340/robot/Constants.java @@ -168,6 +168,8 @@ public static final class ShooterConstants { public static final SparkPIDControllerConfig SHOOT_PID_CONFIG = new SparkPIDControllerConfig().setPID(0.0, 0.0, 0.0, 0); public static final double FEED_INTAKE_SPEED = 0.0; + + public static final String FEED_PID = null; } /** diff --git a/src/main/java/org/team340/robot/subsystems/Shooter.java b/src/main/java/org/team340/robot/subsystems/Shooter.java index 039280d..e4d1c51 100644 --- a/src/main/java/org/team340/robot/subsystems/Shooter.java +++ b/src/main/java/org/team340/robot/subsystems/Shooter.java @@ -12,7 +12,7 @@ import java.util.function.Supplier; import org.team340.lib.GRRSubsystem; import org.team340.lib.util.Mutable; -import org.team340.lib.util.config.rev.SparkPIDControllerConfig; +import org.team340.robot.Constants.PivotConstants; import org.team340.robot.Constants.RobotMap; import org.team340.robot.Constants.ShooterConstants; @@ -38,26 +38,21 @@ public class Shooter extends GRRSubsystem { public Shooter() { super("Shooter"); - new SparkPIDControllerConfig() - .setPID(ShooterConstants.FEED_PID.p(), ShooterConstants.FEED_PID.i(), ShooterConstants.FEED_PID.d()) - .setIZone(ShooterConstants.FEED_PID.iZone()) - .apply(feedMotor, feedPID); + ShooterConstants.FEED_MOTOR_CONFIG.apply(feedMotor); + ShooterConstants.FEED_PID_CONFIG.apply(feedMotor, feedPID); - new SparkPIDControllerConfig() - .setPID(ShooterConstants.LEFT_SHOOT_PID.p(), ShooterConstants.LEFT_SHOOT_PID.i(), ShooterConstants.LEFT_SHOOT_PID.d()) - .setIZone(ShooterConstants.LEFT_SHOOT_PID.iZone()) - .apply(leftShootMotor, leftShootPID); + ShooterConstants.SHOOT_LEFT_MOTOR_CONFIG.apply(leftShootMotor); + ShooterConstants.SHOOT_PID_CONFIG.apply(leftShootMotor, leftShootPID); - new SparkPIDControllerConfig() - .setPID(ShooterConstants.RIGHT_SHOOT_PID.p(), ShooterConstants.RIGHT_SHOOT_PID.i(), ShooterConstants.RIGHT_SHOOT_PID.d()) - .setIZone(ShooterConstants.RIGHT_SHOOT_PID.iZone()) - .apply(rightShootMotor, rightShootPID); + ShooterConstants.SHOOT_RIGHT_MOTOR_CONFIG.apply(leftShootMotor); + ShooterConstants.SHOOT_PID_CONFIG.apply(rightShootMotor, rightShootPID); } /** * This starts running the pivot motor to an angle. This must run regularly until the shooter reaches the angle. * @param angleToShootAt This is the angle that will be used. */ + @Deprecated private void setShooterToAngle(double angleToShootAt) { if (angleToShootAt < PivotConstants.MINIMUM_ANGLE || angleToShootAt > PivotConstants.MAXIMUM_ANGLE) { DriverStation.reportWarning( @@ -71,7 +66,7 @@ private void setShooterToAngle(double angleToShootAt) { ); return; } - pivotPID.setReference(distanceToMoveDart(angleToShootAt), ControlType.kSmartMotion); + // pivotPID.setReference(distanceToMoveDart(angleToShootAt), ControlType.kSmartMotion); } /** @@ -93,9 +88,10 @@ private double dartExtensionFromAngle(double sensorAngle) { * @param targetAngle Pivot angle the dart needs to be at * @return distance the dart needs to be moved */ + @Deprecated private double distanceToMoveDart(double targetAngle) { - double deltaExtensionLength = dartExtensionFromAngle(targetAngle) - dartExtensionFromAngle(pivotAbsoluteEncoder.getPosition()); - return deltaExtensionLength + pivotRelativeEncoder.getPosition(); + // double deltaExtensionLength = dartExtensionFromAngle(targetAngle) - dartExtensionFromAngle(pivotAbsoluteEncoder.getPosition()); + return 0.0; // deltaExtensionLength + pivotRelativeEncoder.getPosition(); } /** @@ -150,8 +146,9 @@ public Command shooterToAngle(double angleToShootAt) { * @param angleToShootAt This is the angle the shooter will go to, check {@link #hasShooterReachedAngle()} to see if it has finished. * @return */ + @Deprecated public Command shooterToAngle(Supplier angleToShootAt) { - return commandBuilder().onExecute(() -> setShooterToAngle(angleToShootAt.get())).onEnd(() -> pivotMotor.stopMotor()); + return commandBuilder().onExecute(() -> setShooterToAngle(angleToShootAt.get())).onEnd(() -> {}/*pivotMotor.stopMotor()*/); } /** @@ -176,6 +173,7 @@ public Command setShootSpeed(Supplier shooterSpeed) { * @param robotPosition This is the position used for the math, note that it must be a supplier. * @return This command. */ + @Deprecated public Command shootSpeaker(Supplier robotPosition) { Mutable counter = new Mutable<>(0); @@ -196,13 +194,14 @@ public Command shootSpeaker(Supplier robotPosition) { }) .isFinished(() -> counter.get() == 0) .onEnd(() -> { - pivotMotor.stopMotor(); + // pivotMotor.stopMotor(); feedMotor.stopMotor(); leftShootMotor.stopMotor(); rightShootMotor.stopMotor(); }); } + @Deprecated public Command shootSpeaker(Pose2d robotPosition) { return shootSpeaker(() -> robotPosition); }